From e36cb7e44611855d39ba84710a7007ca659e9cd5 Mon Sep 17 00:00:00 2001
From: hgn <hgodden00@gmail.com>
Date: Wed, 29 Jun 2022 03:28:34 +0100
Subject: [PATCH] everything

---
 blender_export.py | 347 ++++++++++++++++++++++++++++++----------------
 common.h          |   6 +
 gate.h            | 100 +++----------
 main.c            |  18 +--
 model.h           |  13 ++
 player.h          | 289 +++++++++++++++++++++++++++++++++++---
 rigidbody.h       |  53 ++++++-
 water.h           |  15 ++
 world.h           | 130 ++++++++++++-----
 9 files changed, 699 insertions(+), 272 deletions(-)

diff --git a/blender_export.py b/blender_export.py
index 8fb8bf7..afb0f78 100644
--- a/blender_export.py
+++ b/blender_export.py
@@ -1,5 +1,18 @@
-import bpy, math
+import bpy, math, gpu
 from ctypes import *
+from gpu_extras.batch import batch_for_shader
+
+bl_info = {
+   "name":"Carve exporter",
+   "author": "Harry Godden (hgn)",
+   "version": (0,1),
+   "blender":(3,1,0),
+   "location":"Export",
+   "descriptin":"",
+   "warning":"",
+   "wiki_url":"",
+   "category":"Import/Export",
+}
 
 class model(Structure):
    _pack_ = 1
@@ -21,11 +34,17 @@ class submodel(Structure):
                ("name",c_char*32),
                ("material",c_char*32)]
 
+class classtype_gate(Structure):
+   _pack_ = 1
+   _fields_ = [("target",c_uint32)]
+
 class marker(Structure):
    _pack_ = 1
    _fields_ = [("co",c_float*3),
                ( "q",c_float*4),
                ( "s",c_float*3),
+               ("classtype",c_uint32),
+               ("offset",c_uint32),
                ("name",c_char*32)]
 
 class model_vert(Structure):
@@ -35,114 +54,16 @@ class model_vert(Structure):
                ("colour",c_float*4),
                ("uv",c_float*2)]
 
-def v4_dot( a, b ):
-	return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*a[3]
-
-def v4_length( a ):
-   return math.sqrt( v4_dot(a,a) )
-
-def v2_eq( a, b ):
-   if abs(a[0]-b[0]) < 0.0001:
-      if abs(a[1]-b[1]) < 0.0001:
-         return True
-   return False
-
-def v3_eq( a, b ):
-   if abs(a[0]-b[0]) < 0.0001:
-      if abs(a[1]-b[1]) < 0.0001:
-         if abs(a[2]-b[2]) < 0.0001:
-            return True
-   return False
-
-def v4_eq( a, b ):
-   if abs(a[0]-b[0]) < 0.0001:
-      if abs(a[1]-b[1]) < 0.0001:
-         if abs(a[2]-b[2]) < 0.0001:
-            if abs(a[3]-b[3]) < 0.0001:
-               return True
-   return False
-
-def m3x3_mul( a, b, d ):
-   a00 = a[0][0]
-   a01 = a[0][1]
-   a02 = a[0][2]
-   a10 = a[1][0]
-   a11 = a[1][1]
-   a12 = a[1][2]
-   a20 = a[2][0]
-   a21 = a[2][1]
-   a22 = a[2][2]
-   b00 = b[0][0]
-   b01 = b[0][1]
-   b02 = b[0][2]
-   b10 = b[1][0]
-   b11 = b[1][1]
-   b12 = b[1][2]
-   b20 = b[2][0]
-   b21 = b[2][1]
-   b22 = b[2][2]
-   d[0][0] = a00*b00 + a10*b01 + a20*b02
-   d[0][1] = a01*b00 + a11*b01 + a21*b02
-   d[0][2] = a02*b00 + a12*b01 + a22*b02
-   d[1][0] = a00*b10 + a10*b11 + a20*b12
-   d[1][1] = a01*b10 + a11*b11 + a21*b12
-   d[1][2] = a02*b10 + a12*b11 + a22*b12
-   d[2][0] = a00*b20 + a10*b21 + a20*b22
-   d[2][1] = a01*b20 + a11*b21 + a21*b22
-   d[2][2] = a02*b20 + a12*b21 + a22*b22
-
-def q_m3x3( q, d ):
-   l = v4_length(q)
-   s = 2.0 if l > 0.0 else 0.0
-   xx = s*q[0]*q[0]
-   xy = s*q[0]*q[1]
-   wx = s*q[3]*q[0]
-   yy = s*q[1]*q[1]
-   yz = s*q[1]*q[2]
-   wy = s*q[3]*q[1]
-   zz = s*q[2]*q[2]
-   xz = s*q[0]*q[2]
-   wz = s*q[3]*q[2]
-   d[0][0] = 1.0 - yy - zz
-   d[1][1] = 1.0 - xx - zz
-   d[2][2] = 1.0 - xx - yy
-   d[0][1] = xy + wz
-   d[1][2] = yz + wx
-   d[2][0] = xz + wy
-   d[1][0] = xy - wz
-   d[2][1] = yz - wx
-   d[0][2] = xz - wy
-
-def m3x3_q( m, q ):
-   diag = m[0][0] + m[1][1] + m[2][2]
-   if diag >= 0.0:
-      r    = math.sqrt( 1.0 + diag )
-      rinv = 0.5 / r
-      q[0] = rinv * (m[1][2] - m[2][1])
-      q[1] = rinv * (m[2][0] - m[0][2])
-      q[2] = rinv * (m[0][1] - m[1][0])
-      q[3] = r    * 0.5
-   elif m[0][0] >= m[1][1] and m[0][0] >= m[2][2]:
-      r    = math.sqrt( 1.0 - m[1][1] - m[2][2] + m[0][0] )
-      rinv = 0.5 / r
-      q[0] = r    * 0.5
-      q[1] = rinv * (m[0][1] + m[1][0])
-      q[2] = rinv * (m[0][2] + m[2][0])
-      q[3] = rinv * (m[1][2] - m[2][1])
-   elif m[1][1] >= m[2][2]:
-      r    = math.sqrt( 1.0 - m[0][0] - m[2][2] + m[1][1] )
-      rinv = 0.5 / r
-      q[0] = rinv * (m[0][1] + m[1][0])
-      q[1] = r    * 0.5
-      q[2] = rinv * (m[1][2] + m[2][1])
-      q[3] = rinv * (m[2][0] - m[0][2])
-   else:
-      r    = math.sqrt( 1.0 - m[0][0] - m[1][1] + m[2][2] )
-      rinv = 0.5 / r
-      q[0] = rinv * (m[0][2] + m[2][0])
-      q[1] = rinv * (m[1][2] + m[2][1])
-      q[2] = r    * 0.5
-      q[3] = rinv * (m[0][1] - m[1][0])
+def submesh_set_transform( sm, obj ):
+   sm.pivot[0] =  obj.matrix_world.translation[0]
+   sm.pivot[1] =  obj.matrix_world.translation[2]
+   sm.pivot[2] = -obj.matrix_world.translation[1]
+
+   quat = obj.matrix_world.to_quaternion()
+   sm.q[0] =  quat[1]
+   sm.q[1] =  quat[3]
+   sm.q[2] = -quat[2]
+   sm.q[3] =  quat[0]
 
 def write_model(name):
    fp = open(F"/home/harry/Documents/carve/models/{name}.mdl", "wb")
@@ -153,14 +74,41 @@ def write_model(name):
    header.vertex_count = 0
    header.indice_count = 0
    header.layer_count = 0
-
+   header.marker_count = 1
+   
+   mesh_cache = {}
    layers = []
-   markers = []
    vertex_buffer = []
    indice_buffer = []
 
    print( F"Create mode {name}" )
 
+   rootmarker = marker()
+   rootmarker.co[0] = 0
+   rootmarker.co[1] = 0
+   rootmarker.co[2] = 0
+   rootmarker.q[0] = 0
+   rootmarker.q[1] = 0
+   rootmarker.q[2] = 0
+   rootmarker.q[3] = 1
+   rootmarker.s[0] = 1
+   rootmarker.s[1] = 1
+   rootmarker.s[2] = 1
+   rootmarker.name = "".encode('utf-8')
+   rootmarker.offset = 0
+   rootmarker.classtype = 0
+
+   markers = [ rootmarker ] # aka entities
+   entdata_structs = []
+   entdata_offset = 0
+
+   entity_count = 1
+
+   for obj in collection.objects:
+      if obj.type == 'EMPTY':
+         obj.cv_data.uid = entity_count
+         entity_count += 1
+
    for obj in collection.objects:
       if obj.type == 'EMPTY':
          mk = marker()
@@ -179,31 +127,70 @@ def write_model(name):
          mk.s[1] = obj.scale[2]
          mk.s[2] = obj.scale[1]
          mk.name = obj.name.encode('utf-8')
+         mk.offset = entdata_offset
+
+         classtype = obj.cv_data.classtype
+
+         if classtype == 'k_classtype_gate':
+            mk.classtype = 1
+            entdata_offset += sizeof( classtype_gate )
+
+            gate = classtype_gate()
+            gate.target = 0
+            if obj.cv_data.target != None:
+               gate.target = obj.cv_data.target.cv_data.uid
+
+            entdata_structs += [gate]
+
+         elif classtype == 'k_thingummybob':
+            pass
 
          markers += [mk]
          header.marker_count += 1
 
       elif obj.type == 'MESH':
+         default_mat = c_uint32(69)
+         default_mat.name = ""
+
+         if obj.data.name in mesh_cache:
+            ref = mesh_cache[obj.data.name]
+            for material_id, mref in enumerate(ref['sm']):
+               print(F"  Link submesh({ref['users']}) '{obj.name}:{mat.name}'")
+
+               sm = submodel()
+               sm.indice_start = mref['indice_start']
+               sm.indice_count = mref['indice_count']
+               sm.vertex_start = mref['vertex_start']
+               sm.vertex_count = mref['vertex_count']
+               sm.name = obj.name.encode('utf-8')
+               sm.material = mref['material']
+               sm.bbx = mref['bbx']
+               submesh_set_transform( sm, obj )
+               layers += [sm]
+               header.layer_count += 1
+
+            ref['users'] += 1
+            continue
+
+         ref = mesh_cache[obj.data.name] = {}
+         ref['users'] = 0
+         ref['sm'] = []
+
          dgraph = bpy.context.evaluated_depsgraph_get()
          data = obj.evaluated_get(dgraph).data
          data.calc_loop_triangles()
          data.calc_normals_split()
 
-         for material_id, mat in enumerate(data.materials):
+         mat_list = data.materials if len(data.materials) > 0 else [default_mat]
+         for material_id, mat in enumerate(mat_list):
+            mref = {}
+
             sm = submodel()
             sm.indice_start = header.indice_count
             sm.vertex_start = header.vertex_count
             sm.vertex_count = 0
             sm.indice_count = 0
-            sm.pivot[0] =  obj.matrix_world.translation[0]
-            sm.pivot[1] =  obj.matrix_world.translation[2]
-            sm.pivot[2] = -obj.matrix_world.translation[1]
-
-            quat = obj.matrix_world.to_quaternion()
-            sm.q[0] =  quat[1]
-            sm.q[1] =  quat[3]
-            sm.q[2] = -quat[2]
-            sm.q[3] =  quat[0]
+            submesh_set_transform( sm, obj )
 
             for i in range(3):
                sm.bbx[0][i] =  999999
@@ -274,6 +261,14 @@ def write_model(name):
             header.vertex_count += sm.vertex_count
             header.indice_count += sm.indice_count
 
+            mref['indice_start'] = sm.indice_start
+            mref['indice_count'] = sm.indice_count
+            mref['vertex_start'] = sm.vertex_start
+            mref['vertex_count'] = sm.vertex_count
+            mref['bbx'] = sm.bbx
+            mref['material'] = sm.material
+            ref['sm'] += [mref]
+
    fp.write( bytearray( header ) )
    for l in layers:
       fp.write( bytearray(l) )
@@ -283,8 +278,114 @@ def write_model(name):
       fp.write( bytearray(v) )
    for i in indice_buffer:
       fp.write( bytearray(i) )
+   for ed in entdata_structs:
+      fp.write( bytearray(ed) )
 
    fp.close()
 
-for col in bpy.data.collections["export"].children:
-   write_model( col.name )
+# Clicky clicky GUI
+# ------------------------------------------------------------------------------
+
+cv_view_draw_handler = None
+cv_view_shader = gpu.shader.from_builtin('3D_SMOOTH_COLOR')
+
+def cv_draw():
+   global cv_view_shader
+   cv_view_shader.bind()
+   gpu.state.depth_mask_set(False)
+   gpu.state.line_width_set(2.0)
+   gpu.state.face_culling_set('BACK')
+   gpu.state.depth_test_set('NONE')
+   gpu.state.blend_set('ADDITIVE')
+
+   verts = []
+   colours = []
+
+   for obj in bpy.context.collection.all_objects:
+      if obj.cv_data.classtype == 'k_classtype_gate':
+         if obj.cv_data.target != None:
+            p0 = obj.location
+            p1 = obj.cv_data.target.location
+            verts += [(p0[0],p0[1],p0[2])]
+            verts += [(p1[0],p1[1],p1[2])]
+            colours += [(0,1,0,1.0),(1,0,0,1.0)]
+
+   lines = batch_for_shader(\
+         cv_view_shader, 'LINES', \
+         { "pos":verts, "color":colours })
+
+   lines.draw( cv_view_shader )
+
+def cv_poll_target(scene, obj):
+   if obj == bpy.context.active_object:
+      return False
+   if obj.cv_data.classtype == 'k_classtype_none':
+      return False
+   return True
+
+class CV_OBJ_SETTINGS(bpy.types.PropertyGroup):
+   uid: bpy.props.IntProperty( name="" )
+
+   target: bpy.props.PointerProperty( type=bpy.types.Object, name="target", \
+         poll=cv_poll_target )
+
+   classtype: bpy.props.EnumProperty(
+      name="Format", 
+      items = [
+      ('k_classtype_none', "k_classtype_none", "", 0),
+      ('k_classtype_gate', "k_classtype_gate", "", 1),
+      ])
+
+class CV_OBJ_PANEL(bpy.types.Panel):
+   bl_label="Entity Config"
+   bl_idname="SCENE_PT_cv_entity"
+   bl_space_type='PROPERTIES'
+   bl_region_type='WINDOW'
+   bl_context="object"
+   
+   def draw(_,context):
+      active_object = bpy.context.active_object
+      if active_object == None: return
+      _.layout.prop( active_object.cv_data, "classtype" )
+      _.layout.prop( active_object.cv_data, "target" )
+
+class CV_INTERFACE(bpy.types.Panel):
+   bl_idname = "VIEW3D_PT_carve"
+   bl_label = "Carve"
+   bl_space_type = 'VIEW_3D'
+   bl_region_type = 'UI'
+   bl_category = "Carve"
+
+   def draw(_, context):
+      layout = _.layout
+      layout.operator( "carve.compile_all" )
+
+class CV_COMPILE(bpy.types.Operator):
+   bl_idname="carve.compile_all"
+   bl_label="Compile All"
+
+   def execute(_,context):
+      for col in bpy.data.collections["export"].children:
+         write_model( col.name )
+
+      return {'FINISHED'}
+
+classes = [CV_OBJ_SETTINGS,CV_OBJ_PANEL,CV_COMPILE,CV_INTERFACE]
+
+def register():
+   global cv_view_draw_handler
+
+   for c in classes:
+      bpy.utils.register_class(c)
+
+   bpy.types.Object.cv_data = bpy.props.PointerProperty(type=CV_OBJ_SETTINGS)
+   cv_view_draw_handler = bpy.types.SpaceView3D.draw_handler_add(\
+      cv_draw,(),'WINDOW','POST_VIEW')
+
+def unregister():
+   global cv_view_draw_handler
+
+   for c in classes:
+      bpy.utils.unregister_class(c)
+
+   bpy.types.SpaceView3D.draw_handler_remove(cv_view_draw_handler,'WINDOW')
diff --git a/common.h b/common.h
index cc27202..51eb1fa 100644
--- a/common.h
+++ b/common.h
@@ -7,4 +7,10 @@
 
 static float ktimestep = 1.0f/60.0f;
 
+enum classtype
+{
+   k_classtype_none = 0,
+   k_classtype_gate = 1
+};
+
 #endif /* COMMON_H */
diff --git a/gate.h b/gate.h
index 6a6a0a9..9350d2b 100644
--- a/gate.h
+++ b/gate.h
@@ -18,20 +18,25 @@ grender;
 
 struct teleport_gate
 { 
-   v3f co;
-   v4f q;
+   v3f co[2];
+   v4f q[2];
    v2f dims;
 
-   m4x3f to_world, to_local;
-   teleport_gate *other;
+   m4x3f to_world, recv_to_world, transport;
 };
 
 static void gate_transform_update( teleport_gate *gate )
 {
-   q_m3x3( gate->q, gate->to_world );
-   v3_copy( gate->co, gate->to_world[3] );
+   m4x3f to_local;
+
+   q_m3x3( gate->q[0], gate->to_world );
+   v3_copy( gate->co[0], gate->to_world[3] );
    
-   m4x3_invert_affine( gate->to_world, gate->to_local );
+   m4x3_invert_affine( gate->to_world, to_local );
+
+   q_m3x3( gate->q[1], gate->recv_to_world );
+   v3_copy( gate->co[1], gate->recv_to_world[3] );
+   m4x3_mul( gate->recv_to_world, to_local, gate->transport );
 }
 
 static void gate_register(void)
@@ -60,17 +65,11 @@ static void render_gate( teleport_gate *gate, m4x3f camera )
    m3x3_mulv( camera, (v3f){0.0f,0.0f,-1.0f}, viewdir );
    m3x3_mulv( gate->to_world, (v3f){0.0f,0.0f,-1.0f}, gatedir );
 
-   if( v3_dot(viewdir, gatedir) <= 0.0f )
-      return;
-
    v3f v0;
-   v3_sub( viewpos, gate->co, v0 );
+   v3_sub( viewpos, gate->co[0], v0 );
    if( v3_dot(v0, gatedir) >= 0.0f )
       return;
 
-   m4x3f transport;
-   m4x3_mul( gate->other->to_world, gate->to_local, transport );
-   
    v3f a,b,c,d;
 
    float sx = gate->dims[0],
@@ -85,10 +84,10 @@ static void render_gate( teleport_gate *gate, m4x3f camera )
    vg_line( c,d, 0xffffa000 );
    vg_line( d,a, 0xffffa000 );
 
-   vg_line( gate->co, gate->other->co, 0xffffffff );
+   vg_line2( gate->co[0], gate->co[1], 0xff0000ff, 0x00000000 );
 
    m4x3f cam_new;
-   m4x3_mul( transport, camera, cam_new );
+   m4x3_mul( gate->transport, camera, cam_new );
    
    vg_line_pt3( cam_new[3], 0.3f, 0xff00ff00 );
 
@@ -103,73 +102,12 @@ static void render_gate( teleport_gate *gate, m4x3f camera )
    m4x3_expand( inverse, view );
 
    v4f surface;
-   m3x3_mulv( gate->other->to_world, (v3f){0.0f,0.0f,-1.0f}, surface );
-   surface[3] = v3_dot( surface, gate->other->co );
+   m3x3_mulv( gate->recv_to_world, (v3f){0.0f,0.0f,-1.0f}, surface );
+   surface[3] = v3_dot( surface, gate->co[1] );
 
    m4x4f projection;
    pipeline_projection( projection, 0.1f, 900.0f );
 
-#if 0 /* For debugging frustum */
-   {
-      m4x4f devm;
-      m4x4_mul( projection, view, devm );
-      m4x4_inv( devm, devm );
-      
-      v4f corners[] =
-      {
-         {-1,-1,-1, 1}, { 1,-1,-1, 1}, { 1, 1,-1, 1}, {-1, 1,-1, 1},
-         {-1,-1, 1, 1}, { 1,-1, 1, 1}, { 1, 1, 1, 1}, {-1, 1, 1, 1}
-      };
-
-      for( int i=0; i<vg_list_size(corners); i++ )
-      {
-         m4x4_mulv( devm, corners[i], corners[i] );
-         v3_muls( corners[i], 1.0f/corners[i][3], corners[i] );
-      }
-
-      vg_line( corners[0], corners[1], 0xffffffff );
-      vg_line( corners[1], corners[2], 0xffffffff );
-      vg_line( corners[2], corners[3], 0xffffffff );
-      vg_line( corners[3], corners[0], 0xffffffff );
-      vg_line( corners[4], corners[5], 0xffffffff );
-      vg_line( corners[5], corners[6], 0xffffffff );
-      vg_line( corners[6], corners[7], 0xffffffff );
-      vg_line( corners[7], corners[4], 0xffffffff );
-      vg_line( corners[0], corners[4], 0xffffffff );
-      vg_line( corners[1], corners[5], 0xffffffff );
-      vg_line( corners[2], corners[6], 0xffffffff );
-      vg_line( corners[3], corners[7], 0xffffffff );
-      
-      v3f clipped[4];
-      for( int i=0; i<4; i++ )
-      {
-         v3f v0, c, delta, p0;
-         v3_sub( corners[4+i],corners[0+i], v0 );
-         v3_normalize(v0);
-
-         v3_muls( surface, surface[3], c );
-         v3_sub( c, corners[0+i], delta );
-
-         float t = v3_dot(delta, surface) / v3_dot(surface, v0);
-         v3_muladds( corners[0+i], v0, t, clipped[i] );
-      }
-
-      vg_line( clipped[0], clipped[1], 0xff0000ff );
-      vg_line( clipped[1], clipped[2], 0xff0000ff );
-      vg_line( clipped[2], clipped[3], 0xff0000ff );
-      vg_line( clipped[3], clipped[0], 0xff0000ff );
-
-      m4x3_mulv( gate->other->to_world, (v3f){-2.0f,-2.0f,0.0f}, a );
-      m4x3_mulv( gate->other->to_world, (v3f){ 2.0f,-2.0f,0.0f}, b );
-      m4x3_mulv( gate->other->to_world, (v3f){ 2.0f, 2.0f,0.0f}, c );
-      m4x3_mulv( gate->other->to_world, (v3f){-2.0f, 2.0f,0.0f}, d );
-
-      vg_line( clipped[0], a, 0xff0000ff );
-      vg_line( clipped[1], b, 0xff0000ff );
-      vg_line( clipped[2], c, 0xff0000ff );
-      vg_line( clipped[3], d, 0xff0000ff );
-   }
-#endif
    
    m4x3_mulp( inverse, surface, surface );
    surface[3] = -fabsf(surface[3]);
@@ -216,7 +154,7 @@ static int gate_intersect( teleport_gate *gate, v3f pos, v3f last )
 {
    v4f surface;
    m3x3_mulv( gate->to_world, (v3f){0.0f,0.0f,-1.0f}, surface );
-   surface[3] = v3_dot( surface, gate->co );
+   surface[3] = v3_dot( surface, gate->co[0] );
 
    v3f v0, c, delta, p0;
    v3_sub( pos, last, v0 );
@@ -235,7 +173,7 @@ static int gate_intersect( teleport_gate *gate, v3f pos, v3f last )
       {
          v3f local, rel;
          v3_muladds( last, v0, t, local );
-         v3_sub( gate->co, local, rel );
+         v3_sub( gate->co[0], local, rel );
 
          v3f vup, vside;
          m3x3_mulv( gate->to_world, (v3f){0.0f,1.0f,0.0f}, vup );
diff --git a/main.c b/main.c
index 6b9e4d9..aaf58dc 100644
--- a/main.c
+++ b/main.c
@@ -68,14 +68,6 @@ rigidbody mrs_box = {
    .bbx = {{ -0.5f, -0.25f, -0.25f }, { 0.5f, 0.25f, 0.25f }}
 };
 
-teleport_gate gate_a = {
-   .co = { 0.0f, -3.0f, -15.0f },
-   .q = { 0.0f, 0.0f, 0.0f, 1.0f }
-}, 
-gate_b = {
-   .co = { -8.0f, -3.0f, -17.0f },
-   .q = { 0.0f, 0.0f, 0.0f, 1.0f }
-};
 #endif
 
 static int playermodel( int argc, char const *argv[] )
@@ -205,7 +197,7 @@ void vg_render(void)
    m4x4f world_4x4;
    m4x3_expand( player.camera_inverse, world_4x4 );
 
-   gpipeline.fov = freecam? 60.0f: 100.0f; /* 120 */
+   gpipeline.fov = freecam? 60.0f: 120.0f; /* 120 */
    m4x4_projection( vg_pv, gpipeline.fov, 
          (float)vg_window_x / (float)vg_window_y, 
          0.025f, 1000.0f );
@@ -228,10 +220,12 @@ void vg_render(void)
    glBindFramebuffer( GL_FRAMEBUFFER, 0 );
    render_water_surface( vg_pv );
 
-#if 0
    vg_tex2d_bind( &tex_water, 1 );
-   render_gate( &gate_a, cam_transform );
-#endif
+
+   for( int i=0; i<world.gate_count; i++ )
+   {
+      render_gate( &world.gates[i], player.camera );
+   }
 
    
    /* Copy the RGB of what we have into the background buffer */
diff --git a/model.h b/model.h
index 7194689..430ed26 100644
--- a/model.h
+++ b/model.h
@@ -36,11 +36,18 @@ struct submodel
    char material[32];
 };
 
+struct classtype_gate
+{
+   u32 target;
+};
+
 struct model_marker
 {
    v3f co;
    v4f q;
    v3f s;
+   u32 classtype;
+   u32 offset;
    char name[32];
 };
 
@@ -147,6 +154,12 @@ static u32 *submodel_indice_data( model *mdl, submodel *sub )
    return model_indice_base(mdl) + sub->indice_start;
 }
 
+static void *get_entdata_raw( model *mdl, model_marker *marker )
+{
+   return ((void *)(model_indice_base(mdl) + mdl->indice_count)) + 
+      marker->offset;
+}
+
 static submodel *submodel_get( model *mdl, const char *name )
 {
    for( int i=0; i<mdl->layer_count; i++ )
diff --git a/player.h b/player.h
index 52d658d..790d71c 100644
--- a/player.h
+++ b/player.h
@@ -11,7 +11,7 @@ static int walk_grid_iterations = 1;
 static struct gplayer
 {
    /* Physics */
-   v3f co, v, a, v_last, m, bob;
+   v3f co, v, a, v_last, m, bob, vl;
    v4f rot;
    float vswitch, slip, slip_last,
          reverse;
@@ -46,7 +46,10 @@ static struct gplayer
    v2f angles;
    m4x3f camera, camera_inverse;
 }
-player;
+player = 
+{
+   .on_board = 1
+};
 
 static void player_transform_update(void)
 {
@@ -135,6 +138,11 @@ static void apply_gravity( v3f vel, float const timestep )
    v3_muladds( vel, gravity, timestep, vel );
 }
 
+/*
+ * TODO: The angle bias should become greater when launching from a steeper 
+ *       angle and skewed towords more 'downwards' angles when launching from
+ *       shallower trajectories
+ */
 static void player_start_air(void)
 {
    player.in_air = 1;
@@ -544,32 +552,37 @@ static void player_do_motion(void)
 
    player.iY = 0.0f; /* temp */
 
-#if 0
    /* GATE COLLISION */
-   if( gate_intersect( &gate_a, player.co, prevco ) )
+
+   for( int i=0; i<world.gate_count; i++ )
    {
-      teleport_gate *gate = &gate_a;
-
-      m4x3f transport;
-      m4x3_mul( gate->other->to_world, gate->to_local, transport );
-      m4x3_mulv( transport, player.co, player.co );
-      m3x3_mulv( transport, player.v, player.v );
-      m3x3_mulv( transport, player.v_last, player.v_last );
-      m3x3_mulv( transport, player.m, player.m );
-      m3x3_mulv( transport, player.bob, player.bob );
-
-      v4f transport_rotation;
-      m3x3_q( transport, transport_rotation );
-      q_mul( transport_rotation, player.rot, player.rot );
+      teleport_gate *gate = &world.gates[i];
+
+      if( gate_intersect( gate, player.co, prevco ) )
+      {
+         m4x3_mulv( gate->transport, player.co, player.co );
+         m3x3_mulv( gate->transport, player.v, player.v );
+         m3x3_mulv( gate->transport, player.vl, player.vl );
+         m3x3_mulv( gate->transport, player.v_last, player.v_last );
+         m3x3_mulv( gate->transport, player.m, player.m );
+         m3x3_mulv( gate->transport, player.bob, player.bob );
+
+         v4f transport_rotation;
+         m3x3_q( gate->transport, transport_rotation );
+         q_mul( transport_rotation, player.rot, player.rot );
+
+         break;
+      }
    }
-#endif
 
    /* Camera and character */
    player_transform_update();
+   
+   v3_lerp( player.vl, player.v, 0.05f, player.vl );
 
-   player.angles[0] = atan2f( player.v[0], -player.v[2] );
-   player.angles[1] = atan2f( -player.v[1], sqrtf(player.v[0]*player.v[0]+
-                                       player.v[2]*player.v[2]) ) * 0.3f;
+   player.angles[0] = atan2f(  player.vl[0], -player.vl[2] );
+   player.angles[1] = atan2f( -player.vl[1], sqrtf(player.vl[0]*player.vl[0]+
+                                            player.vl[2]*player.vl[2]) ) * 0.3f;
 
    player.air_blend = vg_lerpf( player.air_blend, player.in_air, 0.04f );
    v3_muladds( player.camera_pos, player.v, -0.05f*player.air_blend, 
@@ -1874,6 +1887,237 @@ static void player_animate(void)
    player.mdl.rhead = rhead;
 }
 
+static int giftwrapXZ( v3f *points, int *output, int len )
+{
+   int l, p, q, count;
+
+   if( len < 3 )
+      return 0;
+
+   l = 0;
+   for( int i=1; i<len; i++ )
+      if( points[i][0] < points[l][0] )
+         l = i;
+
+   p = l;
+   count = 0;
+   do
+   {
+      if( count >= len )
+      {
+         vg_error ("MANIFOLD ERR (%d)\n", count );
+         return 0;
+      }
+      output[ count ++ ] = p;
+
+      q = (p+1)%len;
+
+      for( int i=0; i<len; i++ )
+      {
+         if( (points[i][2]-points[p][2])*(points[q][0]-points[i][0]) -
+             (points[i][0]-points[p][0])*(points[q][2]-points[i][2]) 
+             > 0.0001f )
+         {
+            q = i;
+         }
+      }
+      p = q;
+   }
+   while( p != l );
+   
+   return count;
+}
+ 
+static void player_do_collision( rigidbody *rb )
+{
+   /*
+    * If point is inside box
+    *   find normal (theres 8 simple pyramid regions for this, x>y/dim .. etc)
+    *   find distance (same sorta thing)
+    *
+    * apply normal impulse to rotation
+    * correct position based on new penetration amount if needed
+    * apply normal impulse to velocity
+    */
+
+   v3f pfront, pback;
+   m4x3_mulv( player.to_world, (v3f){ 0.0f,0.0f,-1.0f }, pfront );
+   m4x3_mulv( player.to_world, (v3f){ 0.0f,0.0f, 1.0f }, pback );
+
+   float const kheight = 2.0f;
+   
+   v3f verts[8];
+
+   v3f a, b;
+   v3_copy( rb->bbx[0], a );
+   v3_copy( rb->bbx[1], b );
+
+   m4x3f compound;
+   m4x3_mul( player.to_local, rb->to_world, compound );
+   
+	m4x3_mulv( compound, (v3f){ a[0], a[1], a[2] }, verts[0] );
+	m4x3_mulv( compound, (v3f){ a[0], b[1], a[2] }, verts[1] );
+	m4x3_mulv( compound, (v3f){ b[0], b[1], a[2] }, verts[2] );
+	m4x3_mulv( compound, (v3f){ b[0], a[1], a[2] }, verts[3] );
+
+	m4x3_mulv( compound, (v3f){ a[0], a[1], b[2] }, verts[4] );
+	m4x3_mulv( compound, (v3f){ a[0], b[1], b[2] }, verts[5] );
+	m4x3_mulv( compound, (v3f){ b[0], b[1], b[2] }, verts[6] );
+	m4x3_mulv( compound, (v3f){ b[0], a[1], b[2] }, verts[7] );
+
+   int const indices[12][2] = {
+      {0,1},{1,2},{2,3},{3,0},{4,5},{5,6},{6,7},{7,4},
+      {0,4},{1,5},{2,6},{3,7}
+   };
+   
+   v3f hull[12*2 + 8];
+   int hull_indices[12*2 + 8];
+   int hull_len = 0;
+
+   for( int i=0; i<vg_list_size(indices); i++ )
+   {
+      int ia = indices[i][0],
+          ib = indices[i][1];
+
+      v3f p0, p1;
+
+      float ya = verts[ia][1],
+            yb = verts[ib][1],
+            d = 1.0f/(yb-ya),
+            qa;
+
+      if( (ya-0.2f) * (yb-0.2f) < 0.0f )
+      {
+         v3_muls( verts[ia], (yb-0.2f)*d, p0 );
+         v3_muladds( p0, verts[ib], -(ya-0.2f)*d, p0 );
+         
+         v3_copy( p0, hull[hull_len] );
+         hull[hull_len ++][1] = 0.2f;
+
+         m4x3_mulv( player.to_world, p0, p0 );
+         vg_line_pt3( p0, 0.1f, 0xffffff00 );
+      }
+
+      if( (ya-kheight) * (yb-kheight) < 0.0f )
+      {
+         v3_muls( verts[ia], (yb-kheight)*d, p0 );
+         v3_muladds( p0, verts[ib], -(ya-kheight)*d, p0 );
+
+         v3_copy( p0, hull[hull_len] );
+         hull[hull_len ++][1] = 0.2f;
+
+         m4x3_mulv( player.to_world, p0, p0 );
+         vg_line_pt3( p0, 0.1f, 0xff00ffff );
+      }
+   }
+   for( int i=0; i<8; i++ )
+   {
+      int ia = indices[i][0];
+      float ya = verts[ia][1];
+
+      if( ya > 0.2f && ya < kheight )
+      {
+         v3_copy( verts[ia], hull[hull_len] );
+         hull[hull_len ++][1] = 0.2f;
+      }
+   }
+
+   if( hull_len < 3 ) 
+      return;
+
+   int len = giftwrapXZ( hull, hull_indices, hull_len );
+   for( int i=0; i<len; i++ )
+   {
+      v3f p0, p1, p2, p3;
+      v3_copy( hull[hull_indices[i]], p0 );
+      v3_copy( hull[hull_indices[(i+1)%len]], p1 );
+      v3_add( p0, (v3f){0,kheight-0.2f,0}, p2 );
+      v3_add( p1, (v3f){0,kheight-0.2f,0}, p3 );
+         
+      m4x3_mulv( player.to_world, p0, p0 );
+      m4x3_mulv( player.to_world, p1, p1 );
+      m4x3_mulv( player.to_world, p2, p2 );
+      m4x3_mulv( player.to_world, p3, p3 );
+
+      vg_line2( p0, p1, 0xff00ffff, 0xff000000 );
+      vg_line( p2, p3, 0xff00ffff );
+      vg_line( p0, p2, 0xff00ffa0 );
+   }
+
+   int collide = 1;
+   float min_dist = 99999.9f;
+   v2f normal;
+   for( int i=0; i<len; i++ )
+   {
+      v2f p0, p1;
+      p0[0] = hull[hull_indices[i]][0];
+      p0[1] = hull[hull_indices[i]][2];
+      p1[0] = hull[hull_indices[(i+1)%len]][0];
+      p1[1] = hull[hull_indices[(i+1)%len]][2];
+      
+      v2f t,n, rel;
+      v2_sub( p1, p0, t );
+      n[0] = -t[1];
+      n[1] =  t[0];
+      v2_normalize(n);
+      
+      v2_sub( (v2f){ 0.0f, -1.0f }, p0, rel );
+      float d = -v2_dot( n, rel ) + 0.5f;
+
+      if( d < 0.0f )
+      {
+         collide = 0;
+         break;
+      }
+
+      if( d < min_dist )
+      {
+         min_dist = d;
+         v2_copy( n, normal );
+      }
+   }
+
+   if( collide )
+   {
+      v3f p0, p1;
+      p0[0] =  0.0f;
+      p0[1] =  0.2f;
+      p0[2] = -1.0f;
+
+      p1[0] = p0[0] + normal[0]*min_dist;
+      p1[1] = p0[1];
+      p1[2] = p0[2] + normal[1]*min_dist;
+      
+      m4x3_mulv( player.to_world, p0, p0 );
+      m4x3_mulv( player.to_world, p1, p1 );
+
+      vg_line( p0, p1, 0xffffffff );
+
+      v2f impulse;
+      v2_muls( normal, min_dist, impulse );
+      float rotation = v2_cross( (v2f){0.0f,-1.0f}, impulse )*0.08f;
+      
+      v3f vel;
+      m3x3_mulv( player.to_local, player.v, vel );
+      vel[1] = vel[2];
+
+      float vn = vg_maxf( -v2_dot( vel, normal ), 0.0f );
+      vn += -0.2f * (1.0f/k_rb_delta) * vg_minf( 0.0f, -min_dist+0.04f );
+
+      v2_muls( normal, vn*0.03f, impulse );
+      v3f impulse_world = { impulse[0], 0.0f, impulse[1] };
+
+      m3x3_mulv( player.to_world, impulse_world, impulse_world );
+      v3_add( impulse_world, player.v, player.v );
+      
+      v4f rot;
+      v3f up = {0.0f,1.0f,0.0f};
+      m3x3_mulv( player.to_world, up, up );
+      q_axis_angle( rot, up, -rotation );
+      q_mul( rot, player.rot, player.rot );
+   }
+}
+
 static void player_update(void)
 {
    if( vg_get_axis("grabl")>0.0f)
@@ -1894,6 +2138,9 @@ static void player_update(void)
       {
          if( player.on_board )
          {
+            for( int i=0; i<world.rb_count; i++ )
+               player_do_collision( &world.temp_rbs[i] );
+
             player_do_motion();
             player_animate();
          }
diff --git a/rigidbody.h b/rigidbody.h
index 01734e8..82e7934 100644
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -11,8 +11,6 @@ static void rb_tangent_basis( v3f n, v3f tx, v3f ty );
 
 #define RB_DEPR 
 
-#include "world.h"
-
 #define k_rb_delta (1.0f/60.0f)
 
 typedef struct rigidbody rigidbody;
@@ -108,6 +106,8 @@ static void rb_tangent_basis( v3f n, v3f tx, v3f ty )
    v3_cross( n, tx, ty );
 }
 
+#include "world.h"
+
 static void rb_build_manifold( rigidbody *rb )
 {
    v3f *box = rb->bbx;
@@ -432,4 +432,53 @@ static void rb_debug( rigidbody *rb, u32 colour )
    vg_line( p100, p010, colour );
 }
 
+/*
+ * out penetration distance, normal
+ */
+static int rb_point_in_body( rigidbody *rb, v3f pos, float *pen, v3f normal )
+{
+   v3f local;
+   m4x3_mulv( rb->to_local, pos, local );
+
+   if( local[0] > rb->bbx[0][0] && local[0] < rb->bbx[1][0] &&
+       local[1] > rb->bbx[0][1] && local[1] < rb->bbx[1][1] &&
+       local[2] > rb->bbx[0][2] && local[2] < rb->bbx[1][2] )
+   {
+      v3f area, com, comrel;
+      v3_add( rb->bbx[0], rb->bbx[1], com );
+      v3_muls( com, 0.5f, com );
+
+      v3_sub( rb->bbx[1], rb->bbx[0], area );
+      v3_sub( local, com, comrel );
+      v3_div( comrel, area, comrel );
+
+      int axis = 0;
+      float max_mag = fabsf(comrel[0]);
+      
+      if( fabsf(comrel[1]) > max_mag )
+      {
+         axis = 1;
+         max_mag = fabsf(comrel[1]);
+      }
+      if( fabsf(comrel[2]) > max_mag )
+      {
+         axis = 2;
+         max_mag = fabsf(comrel[2]);
+      }
+      
+      v3_zero( normal );
+      normal[axis] = vg_signf(comrel[axis]);
+
+      if( normal[axis] < 0.0f )
+         *pen = local[axis] - rb->bbx[0][axis];
+      else
+         *pen = rb->bbx[1][axis] - local[axis];
+
+      m3x3_mulv( rb->to_world, normal, normal );
+      return 1;
+   }
+
+   return 0;
+}
+
 #endif /* RIGIDBODY_H */
diff --git a/water.h b/water.h
index 81d709e..83a6cf8 100644
--- a/water.h
+++ b/water.h
@@ -21,6 +21,7 @@ static struct
    int depth_computed;
 
    float height;
+   int enabled;
 }
 wrender;
 
@@ -31,12 +32,17 @@ static void water_register(void)
 
 static void water_init(void)
 {
+   /* TODO: probably dont do this every time */
    create_renderbuffer_std( &wrender.fb, &wrender.rgb, &wrender.rb );
+   wrender.enabled = 1;
 }
 
 static int ray_world( v3f pos, v3f dir, ray_hit *hit );
 static void water_compute_depth( boxf bounds )
 {
+   if( !wrender.enabled )
+      return;
+
 #ifdef VG_RELEASE
    int const kres = 512;
 #else
@@ -100,11 +106,17 @@ static void water_set_surface( glmesh *surf, float height )
 
 static void water_fb_resize(void)
 {
+   if( !wrender.enabled )
+      return;
+
    resize_renderbuffer_std( &wrender.fb, &wrender.rgb, &wrender.rb );
 }
 
 static void render_water_texture( m4x3f camera )
 {
+   if( !wrender.enabled )
+      return;
+
    /* Draw reflection buffa */
    glBindFramebuffer( GL_FRAMEBUFFER, wrender.fb );
    glClearColor( 0.11f, 0.35f, 0.37f, 1.0f );
@@ -150,6 +162,9 @@ static void render_water_texture( m4x3f camera )
 
 static void render_water_surface( m4x4f pv )
 {
+   if( !wrender.enabled )
+      return;
+
    /* Draw surface */
    shader_water_use();
    
diff --git a/world.h b/world.h
index 29e1e71..cfa3d6b 100644
--- a/world.h
+++ b/world.h
@@ -8,6 +8,8 @@
 #include "terrain.h"
 #include "render.h"
 #include "water.h"
+#include "rigidbody.h"
+#include "gate.h"
 
 #include "shaders/standard.h"
 
@@ -18,6 +20,16 @@ static struct gworld
    glmesh skybox;
 
    v3f tutorial;
+
+#if 0
+   rigidbody box;
+#endif
+
+   teleport_gate gates[16];
+   u32 gate_count;
+
+   rigidbody temp_rbs[32];
+   u32 rb_count;
 }
 world;
 
@@ -36,8 +48,6 @@ static void render_world( m4x4f projection, m4x3f camera )
    scene_bind( &world.foliage );
    scene_draw( &world.foliage );
    glEnable(GL_CULL_FACE);
-
-   vg_line_boxf( world.geo.bbx, 0xff00ffff );
 }
 
 static void ray_world_get_tri( ray_hit *hit, v3f tri[3] )
@@ -83,43 +93,56 @@ static void world_load(void)
    /* 
     * TODO: Parametric marker import
     */
-   v3_copy( model_marker_get( mworld, "mp_dev_tutorial" )->co, world.tutorial );
-
-
-   /* GATE DEV */
-#if 0
+   v3_copy( model_marker_get( mworld, "start" )->co, world.tutorial );
+   
+   /* 
+    * Initialize gates
+    */
+   
+   world.gate_count = 0;
+   for( int i=0; i<mworld->marker_count; i++ )
    {
-      model_marker *ga = model_marker_get(mworld,"gate_a"),
-                   *gb = model_marker_get(mworld,"gate_a_recv");
-
-      v3_copy( ga->co, gate_a.co );
-      v3_copy( gb->co, gate_b.co );
-      v4_copy( ga->q, gate_a.q );
-      v4_copy( gb->q, gate_b.q );
-      v2_copy( ga->s, gate_a.dims );
-      v2_copy( gb->s, gate_b.dims );
-
-      gate_a.other = &gate_b;
-      gate_b.other = &gate_a;
+      model_marker *ga = model_get_marker( mworld, i );
+      
+      if( ga->classtype == k_classtype_gate )
+      {
+         struct classtype_gate *data = get_entdata_raw( mworld, ga );
+         
+         if( data->target )
+         {
+            model_marker *gb = model_get_marker( mworld, data->target );
+            
+            teleport_gate *gate = &world.gates[ world.gate_count ++ ];
+            
+            v3_copy( ga->co, gate->co[0] );
+            v3_copy( gb->co, gate->co[1] );
+            v4_copy( ga->q, gate->q[0] );
+            v4_copy( gb->q, gate->q[1] );
+            v2_copy( ga->s, gate->dims );
 
-      gate_transform_update( &gate_a );
-      gate_transform_update( &gate_b );
+            gate_transform_update( gate );
+         }
+      }
    }
-#endif
 
-   /* WATER DEV
-    * again, TODO: parametric import (material)
+   /*
+    * Load water mesh (1 per world)
     */
+   for( int i=0; i<mworld->layer_count; i++ )
    {
-      glmesh surf;
-      submodel *sm = submodel_get(mworld,"mp_dev_water");
-      model_unpack_submodel( mworld, &surf, sm );
-            
-      water_init();
-      water_set_surface( &surf, sm->pivot[1] );
+      submodel *sm = model_get_submodel( mworld, i );
+      if( !strcmp( sm->material, "water" ) )
+      {
+         glmesh surf;
+         model_unpack_submodel( mworld, &surf, sm );
+               
+         water_init();
+         water_set_surface( &surf, sm->pivot[1] );
+
+         break;
+      }
    }
 
-   free( mworld );
    scene_upload( &world.geo );
    bvh_create( &world.geo );
    
@@ -128,6 +151,47 @@ static void world_load(void)
    scene_init( &world.foliage );
    model *mfoliage = vg_asset_read("models/rs_foliage.mdl");
 
+   /*
+    * TODO: Load any other meshes into the foliage scene, and create rbs for
+    *       them.
+    *
+    *       then compute bvh
+    */
+#if 0
+   scene_add_foliage( &world.foliage, mworld, boxtest, world.box.to_world );
+#endif
+   
+
+#if 0
+   submodel *boxtest = submodel_get( mworld, "cubey" );
+
+#endif
+   
+   for( int i=0; i<mworld->layer_count; i++ )
+   {
+      submodel *sm = model_get_submodel( mworld, i );
+      if( !strcmp( sm->material, "surf" ) ||
+          !strcmp( sm->material, "terrain" ) ||
+          !strcmp( sm->material, "water" ) )
+         continue;
+
+      m4x3f transform;
+      q_m3x3( sm->q, transform );
+      v3_copy( sm->pivot, transform[3] );
+      scene_add_foliage( &world.foliage, mworld, sm, transform );
+
+      rigidbody *rb = &world.temp_rbs[ world.rb_count ++ ];
+
+      box_copy( sm->bbx, rb->bbx );
+      v3_copy( sm->pivot, rb->co );
+      rb_init( rb );
+      v4_copy( sm->q, rb->q );
+      rb_update_transform( rb );
+   }
+
+
+   free( mworld );
+
    v3f volume;
    v3_sub( world.geo.bbx[1], world.geo.bbx[0], volume );
    volume[1] = 1.0f;
@@ -167,10 +231,10 @@ static void world_load(void)
             if( vg_randf() < 0.00000006f )
             {
                m3x3_identity( transform );
-               scene_add_foliage( &world.foliage, mfoliage, sm_tree, transform );
+               scene_add_foliage( &world.foliage, mfoliage, sm_tree, transform);
             }
             else
-               scene_add_foliage( &world.foliage, mfoliage, sm_blob, transform );
+               scene_add_foliage( &world.foliage, mfoliage, sm_blob, transform);
          }
       }
    }
-- 
2.25.1