the walk manifold is alive

[carveJwlIkooP6JGAAIwe30JlM.git] / rigidbody.h

diff --git a/rigidbody.h b/rigidbody.h
index cf258b8282ddcefe10839d69a10f895c906b31ae..01734e8f2d74760ab32659165a1a51c633a4785c 100644 (file)
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -1,10 +1,17 @@
-/* SHite fphysics */
+/* 
+ * Resources: Box2D - Erin Catto
+ *            qu3e  - Randy Gaul
+ */
+
+#include "vg/vg.h"
+static void rb_tangent_basis( v3f n, v3f tx, v3f ty );
 
 #ifndef RIGIDBODY_H
 #define RIGIDBODY_H
 
-#include "vg/vg.h"
-#include "scene.h"
+#define RB_DEPR 
+
+#include "world.h"
 
 #define k_rb_delta (1.0f/60.0f)
 
@@ -14,6 +21,7 @@ struct rigidbody
    v3f co, v, I;
    v4f q;
    boxf bbx;
+   float inv_mass;
 
    struct contact
    {
@@ -24,6 +32,7 @@ struct rigidbody
    manifold[4];
    int manifold_count;
 
+   v3f delta;  /* where is the origin of this in relation to a parent body */
    m4x3f to_world, to_local;
 };
 
@@ -42,6 +51,11 @@ static void rb_init( rigidbody *rb )
    v3_zero( rb->v );
    v3_zero( rb->I );
 
+   v3f dims;
+   v3_sub( rb->bbx[1], rb->bbx[0], dims );
+
+   rb->inv_mass = 1.0f/(8.0f*dims[0]*dims[1]*dims[2]);
+
    rb_update_transform( rb );
 }
 
@@ -94,7 +108,7 @@ static void rb_tangent_basis( v3f n, v3f tx, v3f ty )
    v3_cross( n, tx, ty );
 }
 
-static void rb_build_manifold( rigidbody *rb, scene *sc )
+static void rb_build_manifold( rigidbody *rb )
 {
    v3f *box = rb->bbx;
    v3f pts[8]; 
@@ -128,9 +142,16 @@ static void rb_build_manifold( rigidbody *rb, scene *sc )
       struct contact *ct = &rb->manifold[rb->manifold_count];
       
       v3f surface;
-
       v3_copy( point, surface );
-      bvh_scene_sample( sc, surface, ct->n );
+      surface[1] += 4.0f;
+
+      ray_hit hit;
+      hit.dist = INFINITY;
+      if( !ray_world( surface, (v3f){0.0f,-1.0f,0.0f}, &hit ))
+         continue;
+
+      v3_copy( hit.normal, ct->n );
+      v3_copy( hit.pos, surface );
 
       float p = vg_minf( surface[1] - point[1], 1.0f );
 
@@ -158,7 +179,7 @@ static void rb_build_manifold( rigidbody *rb, scene *sc )
 
 static void rb_constraint_manifold( rigidbody *rb )
 {
-   float k_friction = 0.07f;
+   float k_friction = 0.1f;
 
    /* Friction Impulse */
    for( int i=0; i<rb->manifold_count; i++ )
@@ -214,6 +235,52 @@ static void rb_constraint_manifold( rigidbody *rb )
    }
 }
 
+struct rb_angle_limit
+{
+   rigidbody *rba, *rbb;
+   v3f axis;
+   float impulse, bias;
+};
+
+static int rb_angle_limit_force( 
+                                  rigidbody *rba, v3f va, 
+                                  rigidbody *rbb, v3f vb,
+                                  float max )
+{
+   v3f wva, wvb;
+   m3x3_mulv( rba->to_world, va, wva );
+   m3x3_mulv( rbb->to_world, vb, wvb );
+
+   float dt = v3_dot(wva,wvb)*0.999f,
+        ang = fabsf(dt);
+   ang = acosf( dt );
+   if( ang > max )
+   {
+      float correction = max-ang;
+
+      v3f axis;
+      v3_cross( wva, wvb, axis );
+      
+      v4f rotation;
+      q_axis_angle( rotation, axis, -correction*0.25f );
+      q_mul( rotation, rba->q, rba->q );
+
+      q_axis_angle( rotation, axis,  correction*0.25f );
+      q_mul( rotation, rbb->q, rbb->q );
+
+      return 1;
+   }
+
+   return 0;
+}
+
+static void rb_constraint_angle_limit( struct rb_angle_limit *limit )
+{
+
+}
+
+
+RB_DEPR
 static void rb_constraint_angle( rigidbody *rba, v3f va,
                                  rigidbody *rbb, v3f vb, 
                                  float max, float spring )
@@ -248,11 +315,26 @@ static void rb_constraint_angle( rigidbody *rba, v3f va,
    }
 }
 
+static void rb_relative_velocity( rigidbody *ra, v3f lca,
+                                  rigidbody *rb, v3f lcb, v3f rcv )
+{
+   v3f wca, wcb;
+   m3x3_mulv( ra->to_world, lca, wca );
+   m3x3_mulv( rb->to_world, lcb, wcb );
+
+   v3_sub( ra->v, rb->v, rcv );
+
+   v3f rcv_Ra, rcv_Rb;
+   v3_cross( ra->I, wca, rcv_Ra );
+   v3_cross( rb->I, wcb, rcv_Rb );
+   v3_add( rcv_Ra, rcv, rcv );
+   v3_sub( rcv, rcv_Rb, rcv );
+}
+
 static void rb_constraint_position( rigidbody *ra, v3f lca,
                                     rigidbody *rb, v3f lcb )
 {
    /* C = (COa + Ra*LCa) - (COb + Rb*LCb) = 0 */
-
    v3f wca, wcb;
    m3x3_mulv( ra->to_world, lca, wca );
    m3x3_mulv( rb->to_world, lcb, wcb );
@@ -273,19 +355,24 @@ static void rb_constraint_position( rigidbody *ra, v3f lca,
    v3_cross( rb->I, wcb, rcv_Rb );
    v3_add( rcv_Ra, rcv, rcv );
    v3_sub( rcv, rcv_Rb, rcv );
+
+   float nm = 0.5f/(rb->inv_mass + ra->inv_mass);
+
+   float mass_a = 1.0f/ra->inv_mass,
+         mass_b = 1.0f/rb->inv_mass,
+         total_mass = mass_a+mass_b;
    
    v3f impulse;
-   v3_muls( rcv, 0.5f, impulse );
-   v3_add( impulse, rb->v, rb->v );
+   v3_muls( rcv, 1.0f, impulse );
+   v3_muladds( rb->v, impulse, mass_b/total_mass, rb->v );
    v3_cross( wcb, impulse, impulse );
    v3_add( impulse, rb->I, rb->I );
 
-   v3_muls( rcv, -0.5f, impulse );
-   v3_add( impulse, ra->v, ra->v );
+   v3_muls( rcv, -1.0f, impulse );
+   v3_muladds( ra->v, impulse, mass_a/total_mass, ra->v );
    v3_cross( wca, impulse, impulse );
    v3_add( impulse, ra->I, ra->I );
 
-
 #if 0
    v3f impulse;
    v3_muls( delta, 0.5f*spring, impulse );