From 06f283ec6d2bb1b768cfa01b7ccd073d4f5a3bb3 Mon Sep 17 00:00:00 2001
From: hgn <hgodden00@gmail.com>
Date: Thu, 23 Jun 2022 02:02:20 +0100
Subject: [PATCH] walk manifold clipping

---
 player.h | 380 +++++++++++++++++++++++++++++++++++++++++++++++++------
 scene.h  |  50 ++++++++
 2 files changed, 391 insertions(+), 39 deletions(-)

diff --git a/player.h b/player.h
index 377156d..da1c337 100644
--- a/player.h
+++ b/player.h
@@ -576,8 +576,283 @@ static void player_do_motion(void)
          player.camera_pos );
 }
 
+/*
+ * Get a sample at this pole location, will return 1 if the sample is valid,
+ * and pos will be updated to be the intersection location.
+ */
+static int player_walkgrid_samplepole( u32 *geo, int len, v3f pos )
+{
+   v3f p1;
+   v3_copy( pos, p1 );
+   p1[1] -= 10.0f;
+
+   vg_line( pos, p1, 0x20ffffff );
+
+   v3f sample_pos;
+   v3_copy(pos, sample_pos);
+
+   v3f vdir = {0.0f,-1.0f,0.0f};
+   int count = 0;
+
+   ray_hit hit;
+   hit.dist = INFINITY;
+   for( int i=0; i<len; i++ )
+   {
+      u32 *tri = &world.geo.indices[ geo[i] ];
+      count += bvh_ray_tri( &world.geo, tri, sample_pos, vdir, &hit );
+   }
+
+   if( count )
+   {
+      v3f v0, v1;
+      float *pa = world.geo.verts[hit.tri[0]].co,
+            *pb = world.geo.verts[hit.tri[1]].co,
+            *pc = world.geo.verts[hit.tri[2]].co;
+
+      v3_sub( pa, pb, v0 );
+      v3_sub( pc, pb, v1 );
+      v3_cross( v1, v0, hit.normal );
+      v3_normalize( hit.normal );
+      v3_muladds( sample_pos, vdir, hit.dist, pos );
+
+      draw_cross( pos, 0xff00ff00, 0.05f );
+      return count;
+   }
+   else
+      return 0;
+}
+
+static void player_walkgrid_clip(u32 *geo, int len, v3f pos, v3f dir, v3f clip)
+{
+   float max_dist = 0.0f;
+   v3f tri[3];
+   v3f perp;
+   v3_cross( dir,(v3f){0.0f,1.0f,0.0f},perp );
+   v3_copy( pos, clip );
+
+   for( int i=0; i<len; i++ )
+   {
+      u32 *ptri = &world.geo.indices[ geo[i] ];
+      for( int j=0; j<3; j++ )
+         v3_copy( world.geo.verts[ptri[j]].co, tri[j] );
+
+      for( int k=0; k<3; k++ )
+      {
+         int ia = k,
+             ib = (k+1)%3;
+         
+         v3f v0, v1;
+         v3_sub( tri[ia], pos, v0 );
+         v3_sub( tri[ib], pos, v1 );
+
+         if( (dir[2]*v0[0] - dir[0]*v0[2]) *
+             (dir[2]*v1[0] - dir[0]*v1[2]) < 0.0f )
+         {
+            float da = v3_dot(v0,perp),
+                  db = v3_dot(v1,perp),
+                  d  = da-db,
+                  qa = da/d;
+
+            v3f p0;
+            v3_muls( v1, qa, p0 );
+            v3_muladds( p0, v0, 1.0f-qa, p0 );
+
+            float h = v3_dot(p0,dir)/v3_dot(dir,dir);
+
+            if( h >= max_dist && h <= 1.0f )
+            {
+               max_dist = h;
+               v3_copy( p0, clip );
+            }
+         }
+      }
+   }
+
+   v3f clippos;
+   v3_add( pos, clip, clippos );
+   draw_cross( clippos, 0xffffff00, 0.05f );
+}
+
+static void player_walkgrid_getsurface(void)
+{
+   float const k_gridscale = 0.5f;
+   float const k_stepheight = 0.5f;
+   float const k_walkspeed = 6.0f;
+   float const k_miny = 0.6f;
+   float const k_height = 1.78f;
+   int const k_gridamt = 8;
+   float const k_region_size = (float)k_gridamt/2.0f * k_gridscale;
+
+   v3f cell;
+   v3_muls( player.co, 1.0f/k_gridscale, cell );
+   v3_floor( cell, cell );
+   v3_muls( cell, k_gridscale, cell );
+
+   u32 geo[128];
+
+   boxf region;
+   v3_muladds( cell, (v3f){-1.0f,-1.0f,-1.0f}, k_region_size, region[0] );
+   v3_muladds( cell, (v3f){ 1.0f, 1.0f, 1.0f}, k_region_size, region[1] );
+
+   int tri_count = bvh_select_triangles( &world.geo, region, geo, 128 );
+
+   v3f tri[3];
+   for( int i=0; i<tri_count; i++ )
+   {
+      for( int j=0; j<3; j++ )
+         v3_copy( world.geo.verts[ world.geo.indices[geo[i]+j] ].co, tri[j] );
+
+#if 0
+      vg_line( tri[0], tri[1], 0xffa2ff30 );
+      vg_line( tri[1], tri[2], 0xffa2ff30 );
+      vg_line( tri[2], tri[0], 0xffa2ff30 );
+#endif 
+   }
+
+   struct grid_sample
+   {
+      int valid;
+      v3f clip[2];
+      v3f pos;
+   }
+   samples[ k_gridamt ][ k_gridamt ];
+   
+   /* Get surface samples */
+   for( int y=0; y<k_gridamt; y++ )
+   {
+      for( int x=0; x<k_gridamt; x++ )
+      {
+         struct grid_sample *s = &samples[y][x];
+         v3_muladds( region[0], (v3f){ x, 0, y }, k_gridscale, s->pos );
+         s->pos[1] = player.co[1] + k_height;
+
+         s->valid = player_walkgrid_samplepole( geo, tri_count, s->pos )? 1: 0;
+      }
+   }
+   
+   /* 
+    * Calculate h+v clipping distances.
+    * Distances are stored in A always, so you know that if the sample is
+    * invalid, this signifies the start of the manifold as opposed to the
+    * extent or bounds of it.
+    */
+   for( int i=0; i<2; i++ )
+   {
+      for( int x=0; x<k_gridamt; x++ )
+      {
+         for( int z=0; z<k_gridamt-1; z++ )
+         {
+            v3f clipdir = { 0.0f, 0.0f, 0.0f };
+            
+            struct grid_sample *sa, *sb;
+            if( i == 1 )
+            {
+               sa = &samples[z][x];
+               sb = &samples[z+1][x];
+            }
+            else
+            {
+               sa = &samples[x][z];
+               sb = &samples[x][z+1];
+            }
+
+            if( sa->valid != sb->valid )
+            {
+               clipdir[i*2] = (float)(sa->valid - sb->valid)*k_gridscale;
+               player_walkgrid_clip( geo, tri_count, 
+                     sa->valid? sa->pos: sb->pos,
+                     clipdir, sa->clip[i] );
+            }
+            else
+            {
+               if( sa->valid )
+               {
+                  vg_line( sa->pos, sb->pos, 0xffffffff );
+               }
+            }
+         }
+      }
+   }
+
+   /* Draw connections */
+   for( int x=0; x<k_gridamt-1; x++ )
+   {
+      for( int z=0; z<k_gridamt-1; z++ )
+      {
+         static const struct conf
+         {
+            struct confedge
+            {  
+               /* i: sample index
+                * d: data index
+                * a: axis index
+                */
+               int i0, i1, 
+                   d0, d1,
+                   a0, a1;
+            }
+            edges[2];
+            int edge_count;
+         }
+         k_configs[16] = {
+            {{},0},
+            {{{ 3, 3, 3, 0, 1,0 }}, 1},
+            {{{ 2, 2, 1, 3, 0,1 }}, 1},
+            {{{ 2, 3, 1, 0, 0,0 }}, 1},
+
+            {{{ 1, 1, 0, 1, 1,0 }}, 1},
+            {{{ 3, 3, 3, 0, 1,0 },
+              { 1, 1, 0, 1, 1,0 }}, 2},
+            {{{ 1, 2, 0, 3, 1,1 }}, 1},
+            {{{ 1, 3, 0, 0, 1,0 }}, 1},
+
+            {{{ 0, 0, 0, 0, 0,1 }}, 1},
+            {{{ 3, 0, 3, 0, 1,1 }}, 1},
+            {{{ 2, 2, 1, 3, 0,1 },
+              { 0, 0, 0, 0, 0,1 }}, 2},
+            {{{ 2, 0, 1, 0, 0,1 }}, 1},
+
+            {{{ 0, 1, 0, 1, 0,0 }}, 1},
+            {{{ 3, 1, 3, 1, 1,0 }}, 1},
+            {{{ 0, 2, 0, 3, 0,1 }}, 1},
+            {{},0},
+         };
+         
+         struct grid_sample *corners[4] =
+         {
+            &samples[z][x],
+            &samples[z+1][x],
+            &samples[z+1][x+1],
+            &samples[z][x+1]
+         };
+
+         u32 config = (corners[0]->valid<<3) | (corners[1]->valid<<2) | 
+                      (corners[2]->valid<<1) | corners[3]->valid;
+
+         const struct conf *conf = &k_configs[ config ];
+
+         for( int i=0; i<conf->edge_count; i++ )
+         {
+            const struct confedge *edge = &conf->edges[i];
+
+            v3f p0, p1;
+            v3_add( corners[edge->i0]->pos, 
+                    corners[edge->d0]->clip[edge->a0], p0 );
+            v3_add( corners[edge->i1]->pos, 
+                    corners[edge->d1]->clip[edge->a1], p1 );
+            vg_line( p0, p1, 0xff0000ff );
+
+            vg_line( corners[edge->i0]->pos, p0, 0xffffffff );
+            vg_line( corners[edge->i1]->pos, p1, 0xffffffff );
+         }
+      }
+   }
+}
+
 static void player_walkgrid(void)
 {
+   player_walkgrid_getsurface();
+
    float const k_gridscale = 0.5f;
    float const k_stepheight = 0.5f;
    float const k_walkspeed = 6.0f;
@@ -585,6 +860,7 @@ static void player_walkgrid(void)
    float const k_height = 1.78f;
    int const k_gridamt = 8;
 
+#if 0
    v3f cell;
    v3_muls( player.co, 1.0f/k_gridscale, cell );
    v3_floor( cell, cell );
@@ -633,57 +909,83 @@ static void player_walkgrid(void)
    /*
     * Clip grid intersections with triangle edges
     */
-   for( int x=0; x<k_gridamt; x++ )
+   for( int dir=0; dir<2; dir++ )
    {
-      for( int y=0; y<k_gridamt-1; y++ )
+      for( int x=0; x<k_gridamt; x++ )
       {
-         struct grid_sample *sa = &samples[y][x],
-                            *sb = &samples[y+1][x];
-
-         if( (sa->valid != sb->valid) && (sa->valid||sb->valid) )
+         for( int y=0; y<k_gridamt-1; y++ )
          {
-            v3f tri[3];
-            ray_world_get_tri( sa->valid? &sa->hit: &sb->hit, tri );
-
-            v3f sample;
-            v3_muladds( grid_origin, (v3f){ x, 0, y }, 
-                  k_gridscale, sample);
-            
-            /* Clip triangles until we find an edge inside the cell */
-            int axis = 0;
-            float offset = sample[axis==0?0:2],
-                  basis = sample[axis==0?2:0];
+            struct grid_sample *sa, *sb;
 
-            for( int i=0; i<3; i++ )
+            if( dir == 0 )
             {
-               int ia = i,
-                   ib = (i+1)%3;
-               float pa = tri[ia][axis],
-                     pb = tri[ib][axis];
-
-               vg_line( tri[ia],tri[ib],0xffaaaaaa );
-
-               if( (pa-offset)*(pb-offset) > 0.0f )
-                  continue;
-
-               float d = pb-pa,
-                    qa = (offset-pa)/d,
-                     h = qa*tri[ib][2] + (1.0f-qa)*tri[ia][2],
-                     q = (h-basis)/k_gridscale;
+               sa = &samples[y][x];
+               sb = &samples[y+1][x];
+            }
+            else
+            {
+               sa = &samples[x][y];
+               sb = &samples[x][y+1];
+            }
 
-               if( q >= 0.0f && q <= 1.0f )
+            if( (sa->valid != sb->valid) && (sa->valid||sb->valid) )
+            {
+               int line = dir==0? 0:2,
+                   axis = dir==0? 2:0;
+
+               v3f tri[3];
+               ray_world_get_tri( sa->valid? &sa->hit: &sb->hit, tri );
+
+               v3f other = {0,0,0};
+               other[axis] = sa->valid? k_gridscale: -k_gridscale;
+               v3_add( sa->valid? sa->hit.pos: sb->hit.pos, other, other );
+               vg_line( sa->valid? sa->hit.pos: sb->hit.pos, 
+                     other, 0xffffffff );
+
+               v3f sample;
+               if( dir == 0 )
+               v3_muladds( grid_origin, (v3f){ x, 0, y }, k_gridscale, sample);
+               else
+               v3_muladds( grid_origin, (v3f){ y, 0, x }, k_gridscale, sample);
+               
+               /* Clip triangles until we find an edge inside the cell */
+               float offset = sample[line],
+                      basis = sample[axis];
+
+               for( int i=0; i<3; i++ )
                {
-                  float height = qa*tri[ia][1] + (1.0f-qa)*tri[ib][1];
-
-                  v3f intersection = { offset, height, h };
-                  draw_cross( intersection, 0xffff0000, 0.06f );
-                  break;
+                  int ia = i,
+                      ib = (i+1)%3;
+                  float pa = tri[ia][line],
+                        pb = tri[ib][line];
+
+                  vg_line( tri[ia],tri[ib],0xffaaaaaa );
+
+                  if( (pa-offset)*(pb-offset) > 0.0f )
+                     continue;
+
+                  float d = pb-pa,
+                       qa = (offset-pa)/d,
+                        h = qa*tri[ib][axis] + (1.0f-qa)*tri[ia][axis],
+                        q = (h-basis)/k_gridscale;
+
+                  if( q >= 0.0f && q <= 1.0f )
+                  {
+                     float height = qa*tri[ia][1] + (1.0f-qa)*tri[ib][1];
+                     v3f intersection;
+                     if( dir == 0 )
+                        v3_copy( (v3f){ offset, height, h }, intersection );
+                     else
+                        v3_copy( (v3f){ h, height, offset }, intersection );
+                     draw_cross( intersection, 0xffff0000, 0.06f );
+                     break;
+                  }
                }
             }
          }
       }
    }
-
+#endif
    v3f fwd = { -sinf(-player.angles[0]), 0.0f, -cosf(-player.angles[0]) },
        side = { -fwd[2], 0.0f, fwd[0] };
 
diff --git a/scene.h b/scene.h
index ff97000..42ca838 100644
--- a/scene.h
+++ b/scene.h
@@ -1047,6 +1047,56 @@ static int bvh_raycast( scene *s, v3f co, v3f dir, ray_hit *hit )
    return count;
 }
 
+static int bvh_select_triangles( scene *s, boxf box, u32 *triangles, int len )
+{
+   vg_line_boxf( box, 0xffffff00 );
+
+   /* TODO: use this stack system on the raycast function */
+   int count = 0;
+   u32 stack[100];
+   u32 depth = 1;
+
+   stack[0] = 0;
+   stack[1] = s->bvh.nodes[0].il;
+   stack[2] = s->bvh.nodes[0].ir;
+   
+   while(depth)
+   {
+      bvh_node *inode = &s->bvh.nodes[ stack[depth] ];
+      if( box_overlap( inode->bbx, box ) )
+      {
+         if( inode->count )
+         {
+            if( count + inode->count >= len )
+               return count;
+
+            for( u32 i=0; i<inode->count; i++ )
+               triangles[ count ++ ] = (inode->start+i)*3;
+
+            depth --;
+         }
+         else
+         {
+            if( depth+1 >= vg_list_size(stack) )
+            {
+               vg_error( "Maximum stack reached!" );
+               return count;
+            }
+
+            stack[depth] = inode->il;
+            stack[depth+1] = inode->ir;
+            depth ++;
+         }
+      }
+      else
+      {
+         depth --;
+      }
+   }
+
+   return count;
+}
+
 static int bvh_scene_sample_node_h( scene *s, u32 inode, v3f pos, v3f norm )
 {
    bvh_node *node = &s->bvh.nodes[ inode ];
-- 
2.25.1