X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;f=rigidbody.h;h=32753d1099db7102eafa1fb0cfea84a2c714a53a;hb=b0a4fb814d794157c55212191df200915ab99515;hp=2714c430b1c341421228205278594ed287506c06;hpb=d57b7661518800479c00300ce57407378696eec9;p=carveJwlIkooP6JGAAIwe30JlM.git

diff --git a/rigidbody.h b/rigidbody.h
index 2714c43..32753d1 100644
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -63,10 +63,10 @@ struct rigidbody
    boxf bbx, bbx_world;
    float inv_mass;
 
-   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
-               */
+   /* inertia model and inverse world tensor */
+   v3f I;
+   m3x3f iI, iIw;
+
    m4x3f to_world, to_local;
 };
 
@@ -92,7 +92,9 @@ static struct contact
    rigidbody *rba, *rbb;
    v3f co, n;
    v3f t[2];
-   float mass_total, p, bias, norm_impulse, tangent_impulse[2];
+   float p, bias, norm_impulse, tangent_impulse[2],
+         normal_mass, tangent_mass[2];
+
    u32 element_id;
 }
 rb_contact_buffer[256];
@@ -116,6 +118,9 @@ static void rb_update_transform( rigidbody *rb )
    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 );
 
+   m3x3_mul( rb->iI, rb->to_local, rb->iIw );
+   m3x3_mul( rb->to_world, rb->iIw, rb->iIw );
+
    rb_update_bounds( rb );
 }
 
@@ -134,12 +139,17 @@ static void rb_init( rigidbody *rb )
       v3f dims;
       v3_sub( rb->bbx[1], rb->bbx[0], dims );
       volume = dims[0]*dims[1]*dims[2];
+      
+      if( !rb->is_world )
+         vg_info( "Box volume: %f\n", volume );
    }
    else if( rb->type == k_rb_shape_sphere )
    {
       volume = sphere_volume( rb->inf.sphere.radius );
       v3_fill( rb->bbx[0], -rb->inf.sphere.radius );
       v3_fill( rb->bbx[1],  rb->inf.sphere.radius );
+
+      vg_info( "Sphere volume: %f\n", volume );
    }
    else if( rb->type == k_rb_shape_capsule )
    {
@@ -161,11 +171,32 @@ static void rb_init( rigidbody *rb )
    if( rb->is_world )
    {
       rb->inv_mass = 0.0f;
+      v3_zero( rb->I );
+      m3x3_zero(rb->iI);
    }
    else
    {
-      rb->inv_mass = 1.0f/(8.0f*volume);  /* TODO: Things get weird when mass
-                                                   passes a certain point??? */
+      float mass = 2.0f*volume;
+      rb->inv_mass = 1.0f/mass;
+
+      v3f extent;
+      v3_sub( rb->bbx[1], rb->bbx[0], extent );
+      v3_muls( extent, 0.5f, extent );
+
+      /* local intertia tensor */
+      float ex2 = 4.0f*extent[0]*extent[0],
+            ey2 = 4.0f*extent[1]*extent[1],
+            ez2 = 4.0f*extent[2]*extent[2];
+
+      rb->I[0] = ((1.0f/12.0f) * mass * (ey2+ez2));
+      rb->I[1] = ((1.0f/12.0f) * mass * (ex2+ez2));
+      rb->I[2] = ((1.0f/12.0f) * mass * (ex2+ey2));
+
+      m3x3_identity( rb->iI );
+      rb->iI[0][0] = rb->I[0];
+      rb->iI[1][1] = rb->I[1];
+      rb->iI[2][2] = rb->I[2];
+      m3x3_inv( rb->iI, rb->iI );
    }
 
    v3_zero( rb->v );
@@ -176,7 +207,7 @@ static void rb_init( rigidbody *rb )
 
 static void rb_iter( rigidbody *rb )
 {
-   v3f gravity = { 0.0f, -9.6f, 0.0f };
+   v3f gravity = { 0.0f, -9.8f, 0.0f };
    v3_muladds( rb->v, gravity, k_rb_delta, rb->v );
 
    /* intergrate velocity */
@@ -569,8 +600,8 @@ int rb_intersect_planes_1( v4f a, v4f b, v4f c, v3f p )
 static void rb_debug_contact( rb_ct *ct )
 {
    v3f p1;
-   v3_muladds( ct->co, ct->n, 0.2f, p1 );
-   vg_line_pt3( ct->co, 0.1f, 0xff0000ff );
+   v3_muladds( ct->co, ct->n, 0.1f, p1 );
+   vg_line_pt3( ct->co, 0.025f, 0xff0000ff );
    vg_line( ct->co, p1, 0xffffffff );
 }
 
@@ -1077,16 +1108,83 @@ static float rb_box_plane_interval( rigidbody *rba, v4f p )
    return r-s;
 }
 
+static int rb_box_triangle_interval( v3f extent, v3f axis, v3f tri[3] )
+{
+   float 
+
+   r = extent[0] * fabsf(axis[0]) +
+       extent[1] * fabsf(axis[1]) +
+       extent[2] * fabsf(axis[2]),
+
+   p0 = v3_dot( axis, tri[0] ),
+   p1 = v3_dot( axis, tri[1] ),
+   p2 = v3_dot( axis, tri[2] ),
+
+   e = vg_maxf(-vg_maxf(p0,vg_maxf(p1,p2)), vg_minf(p0,vg_minf(p1,p2)));
+
+   if( e > r ) return 0;
+   else return 1;
+}
+
+static int rb_box_triangle_sat( rigidbody *rba, v3f tri_src[3] )
+{
+   v3f tri[3];
+
+   v3f extent, c;
+   v3_sub( rba->bbx[1], rba->bbx[0], extent );
+   v3_muls( extent, 0.5f, extent );
+   v3_add( rba->bbx[0], extent, c );
+
+   for( int i=0; i<3; i++ )
+   {
+      m4x3_mulv( rba->to_local, tri_src[i], tri[i] );
+      v3_sub( tri[i], c, tri[i] );
+   }
+
+   /* u0, u1, u2 */
+   if(!rb_box_triangle_interval( extent, (v3f){1.0f,0.0f,0.0f}, tri )) return 0;
+   if(!rb_box_triangle_interval( extent, (v3f){0.0f,1.0f,0.0f}, tri )) return 0;
+   if(!rb_box_triangle_interval( extent, (v3f){0.0f,0.0f,1.0f}, tri )) return 0;
+
+   v3f v0,v1,v2,n, e0,e1,e2;
+   v3_sub( tri[1], tri[0], v0 );
+   v3_sub( tri[2], tri[0], v1 );
+   v3_sub( tri[2], tri[1], v2 );
+   v3_normalize( v0 );
+   v3_normalize( v1 );
+   v3_normalize( v2 );
+   v3_cross( v0, v1, n );
+   v3_cross( v0, n, e0 );
+   v3_cross( n, v1, e1 );
+   v3_cross( v2, n, e2 );
+
+   /* normal */
+   if(!rb_box_triangle_interval( extent, n, tri )) return 0;
+
+   v3f axis[9];
+   v3_cross( e0, (v3f){1.0f,0.0f,0.0f}, axis[0] );
+   v3_cross( e0, (v3f){0.0f,1.0f,0.0f}, axis[1] );
+   v3_cross( e0, (v3f){0.0f,0.0f,1.0f}, axis[2] );
+   v3_cross( e1, (v3f){1.0f,0.0f,0.0f}, axis[3] );
+   v3_cross( e1, (v3f){0.0f,1.0f,0.0f}, axis[4] );
+   v3_cross( e1, (v3f){0.0f,0.0f,1.0f}, axis[5] );
+   v3_cross( e2, (v3f){1.0f,0.0f,0.0f}, axis[6] );
+   v3_cross( e2, (v3f){0.0f,1.0f,0.0f}, axis[7] );
+   v3_cross( e2, (v3f){0.0f,0.0f,1.0f}, axis[8] );
+   
+   for( int i=0; i<9; i++ )
+      if(!rb_box_triangle_interval( extent, axis[i], tri )) return 0;
+
+   return 1;
+}
+
 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;
 
@@ -1097,123 +1195,127 @@ static int rb_box_vs_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
       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 );
+      if( rb_box_triangle_sat( rba, tri ) )
+      {
+         vg_line(tri[0],tri[1],0xff50ff00 );
+         vg_line(tri[1],tri[2],0xff50ff00 );
+         vg_line(tri[2],tri[0],0xff50ff00 );
+      }
+      else
+      {
+         vg_line(tri[0],tri[1],0xff0000ff );
+         vg_line(tri[1],tri[2],0xff0000ff );
+         vg_line(tri[2],tri[0],0xff0000ff );
 
-      //count += rb_sphere_vs_triangle( rba, rbb, tri, buf+count );
-      //
-      
-      /* TODO: SAT test first */
-      
-      /* 
-       * each pair of faces on the box vs triangle normal
-       */
+         continue;
+      }
 
-      v3f v0, v1;
-      v4f tn;
+      v3f v0,v1,n;
+      v3_sub( tri[1], tri[0], v0 );
+      v3_sub( tri[2], tri[0], v1 );
+      v3_cross( v0, v1, n );
+      v3_normalize( n );
 
-      v3_sub( tri[1],tri[0], v0 );
-      v3_sub( tri[2],tri[0], v1 );
-      v3_cross( v0, v1, tn );
-      v3_normalize( tn );
+      /* find best feature */
+      float best = v3_dot( rba->right, n );
+      int axis = 0;
       
-      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++ )
+      float cy = v3_dot( rba->up, n );
+      if( fabsf(cy) > fabsf(best) )
       {
-         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 ++;
-            }
-         }
+         best = cy;
+         axis = 1;
+      }
+      
+      /* TODO: THIS IS WRONG DIRECTION */
+      float cz = -v3_dot( rba->forward, n );
+      if( fabsf(cz) > fabsf(best) )
+      {
+         best = cz;
+         axis = 2;
       }
-   }
-#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];
+      v3f manifold[4];
 
-   int count = 0;
-   for( int i=0; i<8; i++ )
-   {
-      m4x3_mulv( rba->to_world, pts[i], pts[i] );
+      if( axis == 0 )
+      {
+         float px = best > 0.0f? rba->bbx[0][0]: rba->bbx[1][0];
+         manifold[0][0] = px;
+         manifold[0][1] = rba->bbx[0][1];
+         manifold[0][2] = rba->bbx[0][2];
+         manifold[1][0] = px;
+         manifold[1][1] = rba->bbx[1][1];
+         manifold[1][2] = rba->bbx[0][2];
+         manifold[2][0] = px;
+         manifold[2][1] = rba->bbx[1][1];
+         manifold[2][2] = rba->bbx[1][2];
+         manifold[3][0] = px;
+         manifold[3][1] = rba->bbx[0][1];
+         manifold[3][2] = rba->bbx[1][2];
+      }
+      else if( axis == 1 )
+      {
+         float py = best > 0.0f? rba->bbx[0][1]: rba->bbx[1][1];
+         manifold[0][0] = rba->bbx[0][0];
+         manifold[0][1] = py;
+         manifold[0][2] = rba->bbx[0][2];
+         manifold[1][0] = rba->bbx[1][0];
+         manifold[1][1] = py;
+         manifold[1][2] = rba->bbx[0][2];
+         manifold[2][0] = rba->bbx[1][0];
+         manifold[2][1] = py;
+         manifold[2][2] = rba->bbx[1][2];
+         manifold[3][0] = rba->bbx[0][0];
+         manifold[3][1] = py;
+         manifold[3][2] = rba->bbx[1][2];
+      }
+      else
+      {
+         float pz = best > 0.0f? rba->bbx[0][2]: rba->bbx[1][2];
+         manifold[0][0] = rba->bbx[0][0];
+         manifold[0][1] = rba->bbx[0][1];
+         manifold[0][2] = pz;
+         manifold[1][0] = rba->bbx[1][0];
+         manifold[1][1] = rba->bbx[0][1];
+         manifold[1][2] = pz;
+         manifold[2][0] = rba->bbx[1][0];
+         manifold[2][1] = rba->bbx[1][1];
+         manifold[2][2] = pz;
+         manifold[3][0] = rba->bbx[0][0];
+         manifold[3][1] = rba->bbx[1][1];
+         manifold[3][2] = pz;
+      }
+   
+      for( int j=0; j<4; j++ )
+         m4x3_mulv( rba->to_world, manifold[j], manifold[j] );
 
-      vg_line_pt3( pts[i], 0.1f, 0xffff00ff );
+      vg_line( manifold[0], manifold[1], 0xffffffff );
+      vg_line( manifold[1], manifold[2], 0xffffffff );
+      vg_line( manifold[2], manifold[3], 0xffffffff );
+      vg_line( manifold[3], manifold[0], 0xffffffff );
 
-      if( pts[i][1] < 0.0f )
+      for( int j=0; j<4; j++ )
       {
          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];
+         v3_copy( manifold[j], ct->co );
+         v3_copy( n, ct->n );
+
+         float l0 = v3_dot( tri[0], n ),
+               l1 = v3_dot( manifold[j], n );
+
+         ct->p = (l0-l1)*0.5f;
+         if( ct->p < 0.0f )
+            continue;
+
          ct->rba = rba;
          ct->rbb = rbb;
          count ++;
+
+         if( count >= 12 )
+            return count;
       }
    }
-
-#endif
-
    return count;
 }
 
@@ -1516,13 +1618,42 @@ static void rb_presolve_contacts( rb_ct *buffer, int len )
    for( int i=0; i<len; i++ )
    {
       rb_ct *ct = &buffer[i];
-      ct->bias = -0.2f * k_rb_rate * vg_minf(0.0f,-ct->p+0.04f);
+      ct->bias = -0.2f * k_rb_rate * vg_minf(0.0f,-ct->p+0.01f);
       rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
 
       ct->norm_impulse = 0.0f;
       ct->tangent_impulse[0] = 0.0f;
       ct->tangent_impulse[1] = 0.0f;
-      ct->mass_total = 1.0f/(ct->rba->inv_mass + ct->rbb->inv_mass);
+
+      v3f ra, rb, raCn, rbCn, raCt, rbCt;
+      v3_sub( ct->co, ct->rba->co, ra );
+      v3_sub( ct->co, ct->rbb->co, rb );
+      v3_cross( ra, ct->n, raCn );
+      v3_cross( rb, ct->n, rbCn );
+      
+      /* orient inverse inertia tensors */
+      v3f raCnI, rbCnI;
+      m3x3_mulv( ct->rba->iIw, raCn, raCnI );
+      m3x3_mulv( ct->rbb->iIw, rbCn, rbCnI );
+
+      ct->normal_mass  = ct->rba->inv_mass + ct->rbb->inv_mass;
+      ct->normal_mass += v3_dot( raCn, raCnI );
+      ct->normal_mass += v3_dot( rbCn, rbCnI );
+      ct->normal_mass  = 1.0f/ct->normal_mass;
+
+      for( int j=0; j<2; j++ )
+      {
+         v3f raCtI, rbCtI;
+         v3_cross( ct->t[j], ra, raCt );
+         v3_cross( ct->t[j], rb, rbCt );
+         m3x3_mulv( ct->rba->iIw, raCt, raCtI );
+         m3x3_mulv( ct->rbb->iIw, rbCt, rbCtI );
+         
+         ct->tangent_mass[j]  = ct->rba->inv_mass + ct->rbb->inv_mass;
+         ct->tangent_mass[j] += v3_dot( raCt, raCtI );
+         ct->tangent_mass[j] += v3_dot( rbCt, rbCtI );
+         ct->tangent_mass[j]  = 1.0f/ct->tangent_mass[j];
+      }
 
       rb_debug_contact( ct );
    }
@@ -1556,18 +1687,17 @@ static void rb_standard_impulse( rb_ct *ct, v3f da, v3f db, v3f impulse )
    rigidbody *rba = ct->rba,
              *rbb = ct->rbb;
 
-   v3f ia, ib;
-   v3_muls( impulse,  ct->mass_total*rba->inv_mass, ia );
-   v3_muls( impulse, -ct->mass_total*rbb->inv_mass, ib );
-
-   /* response */
-   v3_add( rba->v, ia, rba->v );
-   v3_add( rbb->v, ib, rbb->v );
+   v3_muladds( rba->v, impulse,  rba->inv_mass, rba->v );
+   v3_muladds( rbb->v, impulse, -rbb->inv_mass, rbb->v );
    
    /* Angular velocity */
-   v3f wa, wb;
-   v3_cross( da, ia, wa );
-   v3_cross( db, ib, wb );
+   v3f wa, wb, invim;
+   v3_cross( da, impulse, wa );
+   v3_negate( impulse, invim );
+   v3_cross( db, invim, wb );
+
+   m3x3_mulv( ct->rba->iIw, wa, wa );
+   m3x3_mulv( ct->rbb->iIw, wb, wb );
    v3_add( rba->w, wa, rba->w );
    v3_add( rbb->w, wb, rbb->w );
 }
@@ -1577,9 +1707,8 @@ 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.5f;
+   float k_friction = 0.2f;
 
-   /* Friction Impulse */
    for( int i=0; i<len; i++ )
    {
       struct contact *ct = &buf[i];
@@ -1588,39 +1717,33 @@ static void rb_solve_contacts( rb_ct *buf, int len )
       v3f rv, da, db;
       rb_rcv( ct, rv, da, db );
       
+      /* Friction */
       for( int j=0; j<2; j++ )
       {
-         float f = k_friction * ct->norm_impulse,
-              vt = -v3_dot( rv, ct->t[j] );
+         float     f = k_friction * ct->norm_impulse,
+                  vt = v3_dot( rv, ct->t[j] ),
+              lambda = ct->tangent_mass[j] * -vt;
          
          float temp = ct->tangent_impulse[j];
-         ct->tangent_impulse[j] = vg_clampf( temp+vt, -f, f );
-         vt = ct->tangent_impulse[j] - temp;
+         ct->tangent_impulse[j] = vg_clampf( temp + lambda, -f, f );
+         lambda = ct->tangent_impulse[j] - temp;
 
          v3f impulse;
-         v3_muls( ct->t[j], vt, impulse );
+         v3_muls( ct->t[j], lambda, impulse );
          rb_standard_impulse( ct, da, db, impulse );
       }
-   }
-
-   /* Normal Impulse */
-   for( int i=0; i<len; i++ )
-   {
-      struct contact *ct = &buf[i];
-      rigidbody *rba = ct->rba,
-                *rbb = ct->rbb;
 
-      v3f rv, da, db;
+      /* Normal */
       rb_rcv( ct, rv, da, db );
-
-      float vn = -v3_dot( rv, ct->n ) + ct->bias;
+      float     vn = v3_dot( rv, ct->n ),
+            lambda = ct->normal_mass * (-vn + ct->bias);
 
       float temp = ct->norm_impulse;
-      ct->norm_impulse = vg_maxf( temp + vn, 0.0f );
-      vn = ct->norm_impulse - temp;
+      ct->norm_impulse = vg_maxf( temp + lambda, 0.0f );
+      lambda = ct->norm_impulse - temp;
 
       v3f impulse;
-      v3_muls( ct->n, vn, impulse );
+      v3_muls( ct->n, lambda, impulse );
       rb_standard_impulse( ct, da, db, impulse );
    }
 }