plish

[carveJwlIkooP6JGAAIwe30JlM.git] / rigidbody.h

diff --git a/rigidbody.h b/rigidbody.h
index d2ce94922cf494335774e918681562f1a40aaab2..3cb601dd705e3ad6e4bb5cf980e9dc22bdd3c5ac 100644 (file)
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -404,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 );
@@ -434,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
  */
@@ -1829,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;