a simple CCD algortihm for walking

[carveJwlIkooP6JGAAIwe30JlM.git] / rigidbody.h

diff --git a/rigidbody.h b/rigidbody.h
index 5c497f21ada170c53a4d7850095b54beffcd025f..d2ce94922cf494335774e918681562f1a40aaab2 100644 (file)
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -169,6 +169,11 @@ struct rb_constr_swingtwist
    float tangent_mass, axis_mass;
 };
 
+struct rb_constr_spring
+{
+   int nothing;
+};
+
 /*
  * -----------------------------------------------------------------------------
  *                               Math Utils
@@ -213,7 +218,7 @@ VG_STATIC void rb_debug_contact( rb_ct *ct )
    {
       v3f p1;
       v3_muladds( ct->co, ct->n, 0.05f, p1 );
-      vg_line_pt3( ct->co, 0.0025f, 0xff0000ff );
+      vg_line_pt3( ct->co, 0.0125f, 0xff0000ff );
       vg_line( ct->co, p1, 0xffffffff );
    }
 }
@@ -1276,6 +1281,7 @@ VG_STATIC int rb_sphere_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
 
 //#define RIGIDBODY_DYNAMIC_MESH_EDGES
 
+__attribute__ ((deprecated))
 VG_STATIC int rb_sphere_triangle( rigidbody *rba, rigidbody *rbb, 
                                   v3f tri[3], rb_ct *buf )
 {
@@ -1323,6 +1329,45 @@ VG_STATIC int rb_sphere_triangle( rigidbody *rba, rigidbody *rbb,
    return 0;
 }
 
+VG_STATIC int rb_sphere__triangle( m4x3f mtxA, rb_sphere *b,
+                                   v3f tri[3], rb_ct *buf )
+{
+   v3f delta, co;
+   enum contact_type type = closest_on_triangle_1( mtxA[3], tri, co );
+
+   v3_sub( mtxA[3], co, delta );
+
+   float d2 = v3_length2( delta ),
+          r = b->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 );
+
+      if( v3_length2( ct->n ) <= 0.00001f )
+      {
+         vg_error( "Zero area triangle!\n" );
+         return 0;
+      }
+
+      v3_normalize( ct->n );
+
+      float d = sqrtf(d2);
+
+      v3_copy( co, ct->co );
+      ct->type = type;
+      ct->p = r-d;
+      return 1;
+   }
+
+   return 0;
+}
 
 VG_STATIC void rb_debug_sharp_scene_edges( rigidbody *rbb, float sharp_ang,
                                            boxf box, u32 colour )
@@ -1418,6 +1463,50 @@ VG_STATIC void rb_debug_sharp_scene_edges( rigidbody *rbb, float sharp_ang,
    }
 }
 
+VG_STATIC int rb_sphere__scene( m4x3f mtxA, rb_sphere *b,
+                                m4x3f mtxB, rb_scene *s, rb_ct *buf )
+{
+   scene *sc = s->bh_scene->user;
+
+   bh_iter it;
+   bh_iter_init( 0, &it );
+   int idx;
+
+   int count = 0;
+
+   float r = b->radius;
+   boxf box;
+   v3_sub( mtxA[3], (v3f){ r,r,r }, box[0] );
+   v3_add( mtxA[3], (v3f){ r,r,r }, box[1] );
+   
+   while( bh_next( s->bh_scene, &it, box, &idx ) )
+   {
+      u32 *ptri = &sc->arrindices[ idx*3 ];
+      v3f tri[3];
+
+      for( int j=0; j<3; j++ )
+         v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
+      
+      buf[ count ].element_id = ptri[0];
+
+      vg_line( tri[0],tri[1],0x70ff6000 );
+      vg_line( tri[1],tri[2],0x70ff6000 );
+      vg_line( tri[2],tri[0],0x70ff6000 );
+
+      int contact = rb_sphere__triangle( mtxA, b, tri, &buf[count] );
+      count += contact;
+
+      if( count == 16 )
+      {
+         vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
+         return count;
+      }
+   }
+
+   return count;
+}
+
+__attribute__ ((deprecated))
 VG_STATIC int rb_sphere_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
 {
    scene *sc = rbb->inf.scene.bh_scene->user;