X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=rigidbody.h;h=2714c430b1c341421228205278594ed287506c06;hb=ef20eba54785473caa827c09acf27607f927b990;hp=b140cbb724068c8c65e3b1f6465afd344d9225ea;hpb=344f0153cf1907da87dd041db3ec517325b1c429;p=carveJwlIkooP6JGAAIwe30JlM.git

diff --git a/rigidbody.h b/rigidbody.h
index b140cbb..2714c43 100644
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -5,6 +5,7 @@
 
 #include "common.h"
 #include "bvh.h"
+#include "scene.h"
 
 static void rb_tangent_basis( v3f n, v3f tx, v3f ty );
 static bh_system bh_system_rigidbodies;
@@ -12,7 +13,7 @@ static bh_system bh_system_rigidbodies;
 #ifndef RIGIDBODY_H
 #define RIGIDBODY_H
 
-#define RB_DEPR 
+//#define RB_DEPR 
 #define k_rb_rate  60.0f
 #define k_rb_delta (1.0f/k_rb_rate)
 
@@ -26,9 +27,10 @@ struct rigidbody
 
    enum rb_shape
    {
-      k_rb_shape_box,
-      k_rb_shape_sphere,
-      k_rb_shape_capsule
+      k_rb_shape_box     = 0,
+      k_rb_shape_sphere  = 1,
+      k_rb_shape_capsule = 2,
+      k_rb_shape_scene   = 3
    } 
    type;
    
@@ -45,6 +47,12 @@ struct rigidbody
          float height, radius;
       } 
       capsule;
+
+      struct rb_scene
+      {
+         scene *pscene;
+      }
+      scene;
    }
    inf;
 
@@ -55,10 +63,17 @@ struct rigidbody
    boxf bbx, bbx_world;
    float inv_mass;
 
-   v3f delta;  /* where is the origin of this in relation to a parent body */
+   v3f delta;  /* where is the origin of this in relation to a parent body
+                  TODO: Move this somewhere other than rigidbody struct
+                        it is only used by character.h's ragdoll
+               */
    m4x3f to_world, to_local;
 };
 
+#ifdef RB_DEPR
+/*
+ * Impulses on static objects get re-routed here
+ */
 static rigidbody rb_static_null = 
 {
    .co={0.0f,0.0f,0.0f},
@@ -68,6 +83,7 @@ static rigidbody rb_static_null =
    .is_world = 1,
    .inv_mass = 0.0f
 };
+#endif
 
 static void rb_debug( rigidbody *rb, u32 colour );
 
@@ -77,10 +93,17 @@ static struct contact
    v3f co, n;
    v3f t[2];
    float mass_total, p, bias, norm_impulse, tangent_impulse[2];
+   u32 element_id;
 }
 rb_contact_buffer[256];
 static int rb_contact_count = 0;
 
+static void rb_update_bounds( rigidbody *rb )
+{
+   box_copy( rb->bbx, rb->bbx_world );
+   m4x3_transform_aabb( rb->to_world, rb->bbx_world );
+}
+
 static void rb_update_transform( rigidbody *rb )
 {
    q_normalize( rb->q );
@@ -89,12 +112,11 @@ static void rb_update_transform( rigidbody *rb )
 
    m4x3_invert_affine( rb->to_world, rb->to_local );
 
-   box_copy( rb->bbx, rb->bbx_world );
-   m4x3_transform_aabb( rb->to_world, rb->bbx_world );
-
    m3x3_mulv( rb->to_world, (v3f){1.0f,0.0f, 0.0f}, rb->right );
    m3x3_mulv( rb->to_world, (v3f){0.0f,1.0f, 0.0f}, rb->up );
    m3x3_mulv( rb->to_world, (v3f){0.0f,0.0f,-1.0f}, rb->forward );
+
+   rb_update_bounds( rb );
 }
 
 static float sphere_volume( float radius )
@@ -130,6 +152,11 @@ static void rb_init( rigidbody *rb )
       rb->bbx[0][1] = -h;
       rb->bbx[1][1] =  h;
    }
+   else if( rb->type == k_rb_shape_scene )
+   {
+      rb->is_world = 1;
+      box_copy( rb->inf.scene.pscene->bbx, rb->bbx );
+   }
 
    if( rb->is_world )
    {
@@ -137,7 +164,8 @@ static void rb_init( rigidbody *rb )
    }
    else
    {
-      rb->inv_mass = 1.0f/(8.0f*volume);
+      rb->inv_mass = 1.0f/(8.0f*volume);  /* TODO: Things get weird when mass
+                                                   passes a certain point??? */
    }
 
    v3_zero( rb->v );
@@ -195,8 +223,10 @@ static void rb_tangent_basis( v3f n, v3f tx, v3f ty )
 }
 
 static void rb_solver_reset(void);
+#ifdef RB_DEPR
 static void rb_build_manifold_terrain( rigidbody *rb );
 static void rb_build_manifold_terrain_sphere( rigidbody *rb );
+#endif
 static void rb_solve_contacts( rb_ct *buf, int len );
 static void rb_presolve_contacts( rb_ct *buffer, int len );
 
@@ -396,6 +426,136 @@ static void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
    v3_muladds( dest, ac, w, dest );
 }
 
+/* TODO */
+static void closest_on_triangle_1( v3f p, v3f tri[3], v3f dest )
+{
+   v3f ab, ac, ap;
+   float d1, d2;
+
+   /* Region outside A */
+   v3_sub( tri[1], tri[0], ab );
+   v3_sub( tri[2], tri[0], ac );
+   v3_sub( p, tri[0], ap );
+   
+   d1 = v3_dot(ab,ap);
+   d2 = v3_dot(ac,ap);
+   if( d1 <= 0.0f && d2 <= 0.0f ) 
+   {
+      v3_copy( tri[0], dest );
+      return;
+   }
+
+   /* Region outside B */
+   v3f bp;
+   float d3, d4;
+
+   v3_sub( p, tri[1], bp );
+   d3 = v3_dot( ab, bp );
+   d4 = v3_dot( ac, bp );
+
+   if( d3 >= 0.0f && d4 <= d3 )
+   {
+      v3_copy( tri[1], dest );
+      return;
+   }
+   
+   /* Edge region of AB */
+   float vc = d1*d4 - d3*d2;
+   if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
+   {
+      float v = d1 / (d1-d3);
+      v3_muladds( tri[0], ab, v, dest );
+      return;
+   }
+
+   /* Region outside C */
+   v3f cp;
+   float d5, d6;
+   v3_sub( p, tri[2], cp );
+   d5 = v3_dot(ab, cp);
+   d6 = v3_dot(ac, cp);
+   
+   if( d6 >= 0.0f && d5 <= d6 )
+   {
+      v3_copy( tri[2], dest );
+      return;
+   }
+
+   /* Region of AC */
+   float vb = d5*d2 - d1*d6;
+   if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
+   {
+      float w = d2 / (d2-d6);
+      v3_muladds( tri[0], ac, w, dest );
+      return;
+   }
+
+   /* Region of BC */
+   float va = d3*d6 - d5*d4;
+   if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
+   {
+      float w = (d4-d3) / ((d4-d3) + (d5-d6));
+      v3f bc;
+      v3_sub( tri[2], tri[1], bc );
+      v3_muladds( tri[1], bc, w, dest );
+      return;
+   }
+
+   /* P inside region, Q via barycentric coordinates uvw */
+   float d = 1.0f/(va+vb+vc),
+         v = vb*d,
+         w = vc*d;
+
+   v3_muladds( tri[0], ab, v, dest );
+   v3_muladds( dest, ac, w, dest );
+}
+
+static int rb_intersect_planes( v4f p0, v4f p1, v4f p2, v3f p )
+{
+   v3f u;
+   v3_cross( p1, p2, u );
+   float d = v3_dot( p0, u );
+
+   if( fabsf(d) < 0.0001f ) 
+      return 0;
+
+   v3_muls( u, p0[3], p );
+
+   v3f v0, v1;
+   v3_muls( p1, p2[3], v0 );
+   v3_muladds( v0, p2, -p1[3], v0 );
+   v3_cross( p0, v0, v1 );
+   v3_add( v1, p, p );
+   v3_muls( p, 1.0f/d, p );
+
+   return 1;
+}
+
+int rb_intersect_planes_1( v4f a, v4f b, v4f c, v3f p )
+{
+   float const epsilon = 0.001;
+        
+   v3f x, bc, ca, ab;
+   float d;
+        
+   v3_cross( a, b, x );
+   d = v3_dot( x, c );
+        
+   if( d < epsilon && d > -epsilon ) return 0;
+        
+   v3_cross(b,c,bc);
+   v3_cross(c,a,ca);
+   v3_cross(a,b,ab);
+
+   v3_muls(       bc, -a[3], p );
+   v3_muladds( p, ca, -b[3], p );
+   v3_muladds( p, ab, -c[3], p );
+
+   v3_negate( p, p );
+   v3_divs( p, d, p );
+        
+   return 1;
+}
 /*
  * Contact generators
  *
@@ -814,6 +974,249 @@ static int rb_sphere_vs_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
    return 0;
 }
 
+/* TODO: these guys */
+
+static int rb_capsule_vs_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+   u32 geo[128];
+   v3f tri[3];
+   int len = bh_select( &rbb->inf.scene.pscene->bhtris, 
+                         rba->bbx_world, geo, 128 );
+
+   return 0;
+}
+
+static int rb_sphere_vs_triangle( rigidbody *rba, rigidbody *rbb, 
+                                  v3f tri[3], rb_ct *buf )
+{
+   v3f delta, co;
+
+   closest_on_triangle( rba->co, tri, co );
+   v3_sub( rba->co, co, delta );
+
+   vg_line( rba->co, co, 0xffff0000 );
+   vg_line_pt3( rba->co, 0.1f, 0xff00ffff );
+
+   float d2 = v3_length2( delta ),
+          r = rba->inf.sphere.radius;
+
+   if( d2 < r*r )
+   {
+      rb_ct *ct = buf;
+
+      v3f ab, ac, tn;
+      v3_sub( tri[2], tri[0], ab );
+      v3_sub( tri[1], tri[0], ac );
+      v3_cross( ac, ab, tn );
+      v3_copy( tn, ct->n );
+      v3_normalize( ct->n );
+
+      float d = sqrtf(d2);
+
+      v3_copy( co, ct->co );
+      ct->p = r-d;
+      ct->rba = rba;
+      ct->rbb = rbb;
+      return 1;
+   }
+
+   return 0;
+}
+
+static int rb_sphere_vs_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+   scene *sc = rbb->inf.scene.pscene;
+   
+   u32 geo[128];
+   v3f tri[3];
+   int len = bh_select( &sc->bhtris, rba->bbx_world, geo, 128 );
+
+   int count = 0;
+
+   for( int i=0; i<len; i++ )
+   {
+      u32 *ptri = &sc->indices[ geo[i]*3 ];
+
+      for( int j=0; j<3; j++ )
+         v3_copy( sc->verts[ptri[j]].co, tri[j] );
+
+      vg_line(tri[0],tri[1],0xff00ff00 );
+      vg_line(tri[1],tri[2],0xff00ff00 );
+      vg_line(tri[2],tri[0],0xff00ff00 );
+      
+      buf[count].element_id = ptri[0];
+      count += rb_sphere_vs_triangle( rba, rbb, tri, buf+count );
+
+      if( count == 12 )
+      {
+         vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
+         return count;
+      }
+   }
+
+   return count;
+}
+
+static float rb_box_plane_interval( rigidbody *rba, v4f p )
+{
+   /* TODO: Make boxes COG aligned as is every other shape. 
+    *       or create COG vector.
+    * TODO: Make forward actually point in the right fucking direction. */
+   v3f e,c;
+   v3_sub( rba->bbx[1], rba->bbx[0], e );
+   v3_muls( e, 0.5f, e );
+   v3_add( rba->bbx[0], e, c );
+   m4x3_mulv( rba->to_world, c, c );
+
+   float r = 
+          e[0]*fabsf( v3_dot(p, rba->right)) +
+          e[1]*fabsf( v3_dot(p, rba->up)) +
+          e[2]*fabsf(-v3_dot(p, rba->forward)),
+   s = v3_dot( p, c ) - p[3];
+   
+   return r-s;
+}
+
+static int rb_box_vs_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+#if 1
+   scene *sc = rbb->inf.scene.pscene;
+   
+   u32 geo[128];
+   v3f tri[3];
+   int len = bh_select( &sc->bhtris, rba->bbx_world, geo, 128 );
+   
+   vg_info( "%d\n", len );
+
+   int count = 0;
+
+   for( int i=0; i<len; i++ )
+   {
+      u32 *ptri = &sc->indices[ geo[i]*3 ];
+
+      for( int j=0; j<3; j++ )
+         v3_copy( sc->verts[ptri[j]].co, tri[j] );
+
+      vg_line(tri[0],tri[1],0xff00ff00 );
+      vg_line(tri[1],tri[2],0xff00ff00 );
+      vg_line(tri[2],tri[0],0xff00ff00 );
+
+      //count += rb_sphere_vs_triangle( rba, rbb, tri, buf+count );
+      //
+      
+      /* TODO: SAT test first */
+      
+      /* 
+       * each pair of faces on the box vs triangle normal
+       */
+
+      v3f v0, v1;
+      v4f tn;
+
+      v3_sub( tri[1],tri[0], v0 );
+      v3_sub( tri[2],tri[0], v1 );
+      v3_cross( v0, v1, tn );
+      v3_normalize( tn );
+      
+      tn[3] = v3_dot( tn, tri[0] );
+
+      v4f pa, pb, pc, pd, pe, pf;
+      v3_copy( rba->right, pa );
+      v3_muls( rba->right, -1.0f, pb );
+      v3_copy( rba->up, pc );
+      v3_muls( rba->up, -1.0f, pd );
+      v3_copy( rba->forward, pf );
+      v3_muls( rba->forward, -1.0f, pe );
+
+      float dx = v3_dot( rba->co, rba->right ),
+            dy = v3_dot( rba->co, rba->up ),
+            dz = -v3_dot( rba->co, rba->forward );
+
+      pa[3] =  dx + rba->bbx[1][0];
+      pb[3] = -dx - rba->bbx[0][0];
+      pc[3] =  dy + rba->bbx[1][1];
+      pd[3] = -dy - rba->bbx[0][1];
+      pe[3] =  dz + rba->bbx[1][2];
+      pf[3] = -dz - rba->bbx[0][2];
+
+      float *pairs[][2] = { {pc, pa}, {pc,pe}, {pc,pb}, {pc,pf},
+                            {pd, pa}, {pd,pe}, {pd,pb}, {pd,pf},
+                            {pf, pa}, {pa,pe}, {pe,pb}, {pb,pf}};
+      for( int j=0; j<vg_list_size(pairs); j++ )
+      {
+         v3f p;
+
+         if( rb_intersect_planes_1( pairs[j][0], pairs[j][1], tn, p ))
+         {
+            v3f p_tri, p_box;
+            closest_point_obb( p, rba, p_box );
+            closest_on_triangle_1( p, tri, p_tri );
+
+            //vg_line_pt3( p, 0.1f, 0xffeeaaff );
+
+            if( v3_dist( p_tri, p ) < 0.001f && v3_dist( p_box, p ) < 0.001f )
+            {
+               if( count == 12 )
+               {
+                  vg_warn( "Exceeding box_vs_scene capacity."
+                           "Geometry too dense!\n" );
+                  return count;
+               }
+
+               rb_ct *ct = buf+count;
+
+               v3_copy( tn, ct->n );
+               v3_copy( p_box, ct->co );
+
+               ct->p = rb_box_plane_interval( rba, tn );
+               ct->rba = rba;
+               ct->rbb = rbb;
+               count ++;
+            }
+         }
+      }
+   }
+#else
+
+   v3f pts[8]; 
+   float *p000 = pts[0], *p001 = pts[1], *p010 = pts[2], *p011 = pts[3],
+         *p100 = pts[4], *p101 = pts[5], *p110 = pts[6], *p111 = pts[7];
+
+   p000[0]=rba->bbx[0][0];p000[1]=rba->bbx[0][1];p000[2]=rba->bbx[0][2];
+   p001[0]=rba->bbx[0][0];p001[1]=rba->bbx[0][1];p001[2]=rba->bbx[1][2];
+   p010[0]=rba->bbx[0][0];p010[1]=rba->bbx[1][1];p010[2]=rba->bbx[0][2];
+   p011[0]=rba->bbx[0][0];p011[1]=rba->bbx[1][1];p011[2]=rba->bbx[1][2];
+   p100[0]=rba->bbx[1][0];p100[1]=rba->bbx[0][1];p100[2]=rba->bbx[0][2];
+   p101[0]=rba->bbx[1][0];p101[1]=rba->bbx[0][1];p101[2]=rba->bbx[1][2];
+   p110[0]=rba->bbx[1][0];p110[1]=rba->bbx[1][1];p110[2]=rba->bbx[0][2];
+   p111[0]=rba->bbx[1][0];p111[1]=rba->bbx[1][1];p111[2]=rba->bbx[1][2];
+
+   int count = 0;
+   for( int i=0; i<8; i++ )
+   {
+      m4x3_mulv( rba->to_world, pts[i], pts[i] );
+
+      vg_line_pt3( pts[i], 0.1f, 0xffff00ff );
+
+      if( pts[i][1] < 0.0f )
+      {
+         rb_ct *ct = buf+count;
+
+         v3_copy( (v3f){0.0f,1.0f,0.0f}, ct->n );
+         v3_copy( pts[i], ct->co );
+         
+         ct->p = 0.0f-pts[i][1];
+         ct->rba = rba;
+         ct->rbb = rbb;
+         count ++;
+      }
+   }
+
+#endif
+
+   return count;
+}
+
 static int RB_MATRIX_ERROR( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
 {
    vg_error( "Collision type is unimplemented between types %d and %d\n",
@@ -837,20 +1240,54 @@ static int rb_box_vs_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
    return rb_sphere_vs_box( rbb, rba, buf );
 }
 
+static int rb_scene_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+   return rb_box_vs_scene( rbb, rba, buf );
+}
+
 static int (*rb_jump_table[4][4])( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
 = {
-              /* box */         /* Sphere */         /* Capsule */
-/*box    */ { RB_MATRIX_ERROR,  rb_box_vs_sphere,    rb_box_vs_capsule,    RB_MATRIX_ERROR },
-/*sphere */ { rb_sphere_vs_box, rb_sphere_vs_sphere, rb_sphere_vs_capsule, RB_MATRIX_ERROR },
+              /* box */         /* Sphere */         /* Capsule */         /* Mesh */
+/*box    */ { RB_MATRIX_ERROR,  rb_box_vs_sphere,    rb_box_vs_capsule,    rb_box_vs_scene },
+/*sphere */ { rb_sphere_vs_box, rb_sphere_vs_sphere, rb_sphere_vs_capsule, rb_sphere_vs_scene },
 /*capsule*/ { rb_capsule_vs_box,rb_capsule_vs_sphere,rb_capsule_vs_capsule,RB_MATRIX_ERROR },
-/*mesh   */ { RB_MATRIX_ERROR,  RB_MATRIX_ERROR,     RB_MATRIX_ERROR,      RB_MATRIX_ERROR }
+/*mesh   */ { rb_scene_vs_box,  RB_MATRIX_ERROR,     RB_MATRIX_ERROR,      RB_MATRIX_ERROR }
 };
 
+static int rb_collide( rigidbody *rba, rigidbody *rbb )
+{
+   int (*collider_jump)(rigidbody *rba, rigidbody *rbb, rb_ct *buf ) 
+      = rb_jump_table[rba->type][rbb->type];
+
+   /* 
+    * 12 is the maximum manifold size we can generate, so we are forced to abort
+    * potentially checking any more.
+    */
+   if( rb_contact_count + 12 > vg_list_size(rb_contact_buffer) )
+   {
+      vg_warn( "Too many contacts made in global collider buffer (%d of %d\n)",
+               rb_contact_count, vg_list_size(rb_contact_buffer) );
+      return 0;
+   }
+
+   /*
+    * TODO: Replace this with a more dedicated broad phase pass
+    */
+   if( box_overlap( rba->bbx_world, rbb->bbx_world ) )
+   {
+      int count = collider_jump( rba, rbb, rb_contact_buffer+rb_contact_count); 
+      rb_contact_count += count;
+      return count;
+   }
+   else
+      return 0;
+}
 
 /*
  * Generic functions
  */
 
+#ifdef RB_DEPR
 /* 
  * This function does not accept triangle as a dynamic object, it is assumed
  * to always be static.
@@ -891,8 +1328,6 @@ static int rb_sphere_vs_triangle( rigidbody *rba, v3f tri[3], rb_ct *buf )
    return 0;
 }
 
-
-RB_DEPR
 static int sphere_vs_triangle( v3f c, float r, v3f tri[3], 
       v3f co, v3f norm, float *p )
 {
@@ -926,6 +1361,7 @@ static int sphere_vs_triangle( v3f c, float r, v3f tri[3],
 }
 
 #include "world.h"
+#endif
 
 static void rb_solver_reset(void)
 {
@@ -937,6 +1373,7 @@ static rb_ct *rb_global_ct(void)
    return rb_contact_buffer + rb_contact_count;
 }
 
+#ifdef RB_DEPR
 static struct contact *rb_start_contact(void)
 {
    if( rb_contact_count == vg_list_size(rb_contact_buffer) )
@@ -1004,7 +1441,6 @@ static void rb_build_manifold_terrain_sphere( rigidbody *rb )
 
 }
 
-RB_DEPR
 static void rb_build_manifold_terrain( rigidbody *rb )
 {
    v3f *box = rb->bbx;
@@ -1069,6 +1505,7 @@ static void rb_build_manifold_terrain( rigidbody *rb )
       }
    }
 }
+#endif
 
 /*
  * Initializing things like tangent vectors
@@ -1140,7 +1577,7 @@ static void rb_standard_impulse( rb_ct *ct, v3f da, v3f db, v3f impulse )
  */
 static void rb_solve_contacts( rb_ct *buf, int len )
 {
-   float k_friction = 0.1f;
+   float k_friction = 0.5f;
 
    /* Friction Impulse */
    for( int i=0; i<len; i++ )
@@ -1235,7 +1672,6 @@ static void rb_constraint_angle_limit( struct rb_angle_limit *limit )
 
 }
 
-RB_DEPR
 static void rb_constraint_angle( rigidbody *rba, v3f va,
                                  rigidbody *rbb, v3f vb, 
                                  float max, float spring )
@@ -1483,8 +1919,13 @@ static void rb_debug( rigidbody *rb, u32 colour )
 
       debug_capsule( rb->to_world, r, h, colour );
    }
+   else if( rb->type == k_rb_shape_scene )
+   {
+      vg_line_boxf( rb->bbx, colour );
+   }
 }
 
+#ifdef RB_DEPR
 /*
  * out penetration distance, normal
  */
@@ -1577,4 +2018,6 @@ static bh_system bh_system_rigidbodies =
    .cast_ray = NULL
 };
 
+#endif
+
 #endif /* RIGIDBODY_H */