plish

[carveJwlIkooP6JGAAIwe30JlM.git] / rigidbody.h

diff --git a/rigidbody.h b/rigidbody.h
index 5c497f21ada170c53a4d7850095b54beffcd025f..3cb601dd705e3ad6e4bb5cf980e9dc22bdd3c5ac 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 );
    }
 }
@@ -399,6 +404,7 @@ VG_STATIC void rb_update_transform( rigidbody *rb )
  * Extrapolate rigidbody into a transform based on vg accumulator.
  * Useful for rendering
  */
+__attribute__ ((deprecated))
 VG_STATIC void rb_extrapolate_transform( rigidbody *rb, m4x3f transform )
 {
    float substep = vg_clampf( vg.accumulator / k_rb_delta, 0.0f, 1.0f );
@@ -429,6 +435,30 @@ VG_STATIC void rb_extrapolate_transform( rigidbody *rb, m4x3f transform )
    v3_copy( co, transform[3] );
 }
 
+VG_STATIC void rb_extrapolate( rigidbody *rb, v3f co, v4f q )
+{
+   float substep = vg_clampf( vg.accumulator / k_rb_delta, 0.0f, 1.0f );
+
+   v3_muladds( rb->co, rb->v, k_rb_delta*substep, co );
+
+   if( v3_length2( rb->w ) > 0.0f )
+   {
+      v4f rotation;
+      v3f axis;
+      v3_copy( rb->w, axis );
+      
+      float mag = v3_length( axis );
+      v3_divs( axis, mag, axis );
+      q_axis_angle( rotation, axis, mag*k_rb_delta*substep );
+      q_mul( rotation, rb->q, q );
+      q_normalize( q );
+   }
+   else
+   {
+      v4_copy( rb->q, q );
+   }
+}
+
 /*
  * Initialize rigidbody and calculate masses, inertia
  */
@@ -1276,6 +1306,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 +1354,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 +1488,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;
@@ -1740,7 +1854,7 @@ VG_STATIC int rb_capsule__scene( m4x3f mtxA, rb_capsule *c,
       int contact = rb_capsule__triangle( mtxA, c, tri, &buf[count] );
       count += contact;
 
-      if( count == 16 )
+      if( count >= 16 )
       {
          vg_warn("Exceeding capsule_vs_scene capacity. Geometry too dense!\n");
          return count;