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

diff --git a/rigidbody.h b/rigidbody.h
index 14bc060..2714c43 100644
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -93,6 +93,7 @@ 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;
@@ -163,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 );
@@ -424,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
  *
@@ -911,7 +1043,8 @@ static int rb_sphere_vs_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
       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 )
@@ -924,6 +1057,166 @@ static int rb_sphere_vs_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
    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",
@@ -947,13 +1240,18 @@ 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 },
+              /* 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 )
@@ -1279,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++ )