X-Git-Url: https://harrygodden.com/git/?a=blobdiff_plain;ds=sidebyside;f=rigidbody.h;h=7f7187daf7aab46e9fc353032b956a42043f6cb5;hb=b4c9550f206c476bb38b0bb2855d35e6b31bee83;hp=32753d1099db7102eafa1fb0cfea84a2c714a53a;hpb=b0a4fb814d794157c55212191df200915ab99515;p=carveJwlIkooP6JGAAIwe30JlM.git

diff --git a/rigidbody.h b/rigidbody.h
index 32753d1..7f7187d 100644
--- a/rigidbody.h
+++ b/rigidbody.h
@@ -184,9 +184,10 @@ static void rb_init( rigidbody *rb )
       v3_muls( extent, 0.5f, extent );
 
       /* local intertia tensor */
-      float ex2 = 4.0f*extent[0]*extent[0],
-            ey2 = 4.0f*extent[1]*extent[1],
-            ez2 = 4.0f*extent[2]*extent[2];
+      float scale = 4.0f;
+      float ex2 = scale*extent[0]*extent[0],
+            ey2 = scale*extent[1]*extent[1],
+            ez2 = scale*extent[2]*extent[2];
 
       rb->I[0] = ((1.0f/12.0f) * mass * (ey2+ez2));
       rb->I[1] = ((1.0f/12.0f) * mass * (ex2+ez2));
@@ -1347,9 +1348,8 @@ 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 */         /* Mesh */
+static int (*rb_jump_table[4][4])( rigidbody *rba, rigidbody *rbb, rb_ct *buf )= 
+{             /* 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 },
@@ -1682,6 +1682,8 @@ static void rb_rcv( rb_ct *ct, v3f rv, v3f da, v3f db )
 /*
  * Apply regular and angular velocity impulses to objects involved in contact
  */
+
+/* TODO REMOVEEE................... */
 static void rb_standard_impulse( rb_ct *ct, v3f da, v3f db, v3f impulse )
 {
    rigidbody *rba = ct->rba,
@@ -1702,6 +1704,20 @@ static void rb_standard_impulse( rb_ct *ct, v3f da, v3f db, v3f impulse )
    v3_add( rbb->w, wb, rbb->w );
 }
 
+/* ......... USE THIS */
+static void rb_linear_impulse( rigidbody *rb, v3f delta, v3f impulse )
+{
+   /* linear */
+   v3_muladds( rb->v, impulse,  rb->inv_mass, rb->v );
+   
+   /* Angular velocity */
+   v3f wa;
+   v3_cross( delta, impulse, wa );
+
+   m3x3_mulv( rb->iIw, wa, wa );
+   v3_add( rb->w, wa, rb->w );
+}
+
 /*
  * One iteration to solve the contact constraint
  */
@@ -1792,7 +1808,7 @@ static int rb_angle_limit_force(  rigidbody *rba, v3f va,
 
 static void rb_constraint_angle_limit( struct rb_angle_limit *limit )
 {
-
+   
 }
 
 static void rb_constraint_angle( rigidbody *rba, v3f va,
@@ -1829,20 +1845,153 @@ static void rb_constraint_angle( rigidbody *rba, v3f va,
    }
 }
 
-static void rb_relative_velocity( rigidbody *ra, v3f lca,
-                                  rigidbody *rb, v3f lcb, v3f rcv )
+static void draw_angle_limit( v3f c, v3f major, v3f minor, 
+                              float amin, float amax, float measured,
+                              u32 colour )
+{
+   float f = 0.05f;
+   v3f ay, ax;
+   v3_muls( major, f, ay );
+   v3_muls( minor, f, ax );
+
+   for( int x=0; x<16; x++ )
+   {
+      float t0 = (float)x / 16.0f,
+            t1 = (float)(x+1) / 16.0f,
+            a0 = vg_lerpf( amin, amax, t0 ),
+            a1 = vg_lerpf( amin, amax, t1 );
+
+      v3f p0, p1;
+      v3_muladds(  c, ay, cosf(a0), p0 );
+      v3_muladds( p0, ax, sinf(a0), p0 );
+      v3_muladds(  c, ay, cosf(a1), p1 );
+      v3_muladds( p1, ax, sinf(a1), p1 );
+      
+      vg_line( p0, p1, colour );
+
+      if( x == 0 )
+         vg_line( c, p0, colour );
+      if( x == 15 )
+         vg_line( c, p1, colour );
+   }
+
+   v3f p2;
+   v3_muladds(  c, ay, cosf(measured)*1.2f, p2 );
+   v3_muladds( p2, ax, sinf(measured)*1.2f, p2 );
+   vg_line( c, p2, colour );
+}
+
+static void rb_debug_constraint_limits( rigidbody *ra, rigidbody *rb, v3f lca,
+                                        v3f limits[2] )
+{
+   v3f ax, ay, az, bx, by, bz;
+   m3x3_mulv( ra->to_world, (v3f){1.0f,0.0f,0.0f}, ax );
+   m3x3_mulv( ra->to_world, (v3f){0.0f,1.0f,0.0f}, ay );
+   m3x3_mulv( ra->to_world, (v3f){0.0f,0.0f,1.0f}, az );
+   m3x3_mulv( rb->to_world, (v3f){1.0f,0.0f,0.0f}, bx );
+   m3x3_mulv( rb->to_world, (v3f){0.0f,1.0f,0.0f}, by );
+   m3x3_mulv( rb->to_world, (v3f){0.0f,0.0f,1.0f}, bz );
+
+   v2f px, py, pz;
+   px[0] = v3_dot( ay, by );
+   px[1] = v3_dot( az, by );
+
+   py[0] = v3_dot( az, bz );
+   py[1] = v3_dot( ax, bz );
+
+   pz[0] = v3_dot( ax, bx );
+   pz[1] = v3_dot( ay, bx );
+
+   float r0 = atan2f( px[1], px[0] ),
+         r1 = atan2f( py[1], py[0] ),
+         r2 = atan2f( pz[1], pz[0] );
+
+   v3f c;
+   m4x3_mulv( ra->to_world, lca, c );
+   draw_angle_limit( c, ay, az, limits[0][0], limits[1][0], r0, 0xff0000ff );
+   draw_angle_limit( c, az, ax, limits[0][1], limits[1][1], r1, 0xff00ff00 );
+   draw_angle_limit( c, ax, ay, limits[0][2], limits[1][2], r2, 0xffff0000 );
+}
+
+static void rb_limit_cure( rigidbody *ra, rigidbody *rb, v3f axis, float d )
+{
+   if( d != 0.0f )
+   {
+      float avx = v3_dot( ra->w, axis ) - v3_dot( rb->w, axis );
+      float joint_mass = rb->inv_mass + ra->inv_mass;
+      joint_mass = 1.0f/joint_mass;
+
+      float bias = (0.04f * k_rb_rate) * d,
+            lambda = -(avx + bias) * joint_mass;
+
+      /* Angular velocity */
+      v3f wa, wb;
+      v3_muls( axis,  lambda * ra->inv_mass, wa );
+      v3_muls( axis, -lambda * rb->inv_mass, wb );
+
+      v3_add( ra->w, wa, ra->w );
+      v3_add( rb->w, wb, rb->w );
+   }
+}
+
+static void rb_constraint_limits( rigidbody *ra, v3f lca, 
+                                  rigidbody *rb, v3f lcb, v3f limits[2] )
 {
+   /* TODO: Code dupe remover */
+   v3f ax, ay, az, bx, by, bz;
+   m3x3_mulv( ra->to_world, (v3f){1.0f,0.0f,0.0f}, ax );
+   m3x3_mulv( ra->to_world, (v3f){0.0f,1.0f,0.0f}, ay );
+   m3x3_mulv( ra->to_world, (v3f){0.0f,0.0f,1.0f}, az );
+   m3x3_mulv( rb->to_world, (v3f){1.0f,0.0f,0.0f}, bx );
+   m3x3_mulv( rb->to_world, (v3f){0.0f,1.0f,0.0f}, by );
+   m3x3_mulv( rb->to_world, (v3f){0.0f,0.0f,1.0f}, bz );
+
+   v2f px, py, pz;
+   px[0] = v3_dot( ay, by );
+   px[1] = v3_dot( az, by );
+
+   py[0] = v3_dot( az, bz );
+   py[1] = v3_dot( ax, bz );
+
+   pz[0] = v3_dot( ax, bx );
+   pz[1] = v3_dot( ay, bx );
+
+   float r0 = atan2f( px[1], px[0] ),
+         r1 = atan2f( py[1], py[0] ),
+         r2 = atan2f( pz[1], pz[0] );
+
+   /* calculate angle deltas */
+   float dx = 0.0f, dy = 0.0f, dz = 0.0f;
+
+   if( r0 < limits[0][0] ) dx = limits[0][0] - r0;
+   if( r0 > limits[1][0] ) dx = limits[1][0] - r0;
+   if( r1 < limits[0][1] ) dy = limits[0][1] - r1;
+   if( r1 > limits[1][1] ) dy = limits[1][1] - r1;
+   if( r2 < limits[0][2] ) dz = limits[0][2] - r2;
+   if( r2 > limits[1][2] ) dz = limits[1][2] - r2;
+
    v3f wca, wcb;
    m3x3_mulv( ra->to_world, lca, wca );
    m3x3_mulv( rb->to_world, lcb, wcb );
 
-   v3_sub( ra->v, rb->v, rcv );
+   rb_limit_cure( ra, rb, ax, dx );
+   rb_limit_cure( ra, rb, ay, dy );
+   rb_limit_cure( ra, rb, az, dz );
+}
 
-   v3f rcv_Ra, rcv_Rb;
-   v3_cross( ra->w, wca, rcv_Ra );
-   v3_cross( rb->w, wcb, rcv_Rb );
-   v3_add( rcv_Ra, rcv, rcv );
-   v3_sub( rcv, rcv_Rb, rcv );
+static void rb_debug_constraint_position( rigidbody *ra, v3f lca,
+                                          rigidbody *rb, v3f lcb )
+{
+   v3f wca, wcb;
+   m3x3_mulv( ra->to_world, lca, wca );
+   m3x3_mulv( rb->to_world, lcb, wcb );
+
+   v3f p0, p1;
+   v3_add( wca, ra->co, p0 );
+   v3_add( wcb, rb->co, p1 );
+   vg_line_pt3( p0, 0.005f, 0xffffff00 );
+   vg_line_pt3( p1, 0.005f, 0xffffff00 );
+   vg_line( p0, p1, 0xffffff00 );
 }
 
 static void rb_constraint_position( rigidbody *ra, v3f lca,
@@ -1853,14 +2002,6 @@ static void rb_constraint_position( rigidbody *ra, v3f lca,
    m3x3_mulv( ra->to_world, lca, wca );
    m3x3_mulv( rb->to_world, lcb, wcb );
 
-   v3f delta;
-   v3_add( wcb, rb->co, delta );
-   v3_sub( delta, wca, delta );
-   v3_sub( delta, ra->co, delta );
-
-   v3_muladds( ra->co, delta,  0.5f, ra->co );
-   v3_muladds( rb->co, delta, -0.5f, rb->co );
-
    v3f rcv;
    v3_sub( ra->v, rb->v, rcv );
 
@@ -1870,41 +2011,47 @@ static void rb_constraint_position( rigidbody *ra, v3f lca,
    v3_add( rcv_Ra, rcv, rcv );
    v3_sub( rcv, rcv_Rb, rcv );
 
-   float nm = 0.5f/(rb->inv_mass + ra->inv_mass);
+   v3f delta;
+   v3f p0, p1;
+   v3_add( wca, ra->co, p0 );
+   v3_add( wcb, rb->co, p1 );
+   v3_sub( p1, p0, delta );
 
-   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, 1.0f, impulse );
-   v3_muladds( rb->v, impulse, mass_b/total_mass, rb->v );
-   v3_cross( wcb, impulse, impulse );
-   v3_add( impulse, rb->w, rb->w );
+   float dist2 = v3_length2( delta );
 
-   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->w, ra->w );
+   if( dist2 > 0.00001f )
+   {
+      float dist = sqrtf(dist2);
+      v3_muls( delta, 1.0f/dist, delta );
 
-#if 0
-   /*
-    * this could be used for spring joints
-    * its not good for position constraint
-    */
-   v3f impulse;
-   v3_muls( delta, 0.5f*spring, impulse );
+      float joint_mass = rb->inv_mass + ra->inv_mass;
 
-   v3_add( impulse, ra->v, ra->v );
-   v3_cross( wca, impulse, impulse );
-   v3_add( impulse, ra->w, ra->w );
+      v3f raCn, rbCn, raCt, rbCt;
+      v3_cross( wca, delta, raCn );
+      v3_cross( wcb, delta, rbCn );
+      
+      /* orient inverse inertia tensors */
+      v3f raCnI, rbCnI;
+      m3x3_mulv( ra->iIw, raCn, raCnI );
+      m3x3_mulv( rb->iIw, rbCn, rbCnI );
+      joint_mass += v3_dot( raCn, raCnI );
+      joint_mass += v3_dot( rbCn, rbCnI );
+      joint_mass = 1.0f/joint_mass;
 
-   v3_muls( delta, -0.5f*spring, impulse );
+      float vd = v3_dot( rcv, delta ),
+            bias = -(0.08f * k_rb_rate) * dist,
+            lambda = -(vd + bias) * joint_mass;
 
-   v3_add( impulse, rb->v, rb->v );
-   v3_cross( wcb, impulse, impulse );
-   v3_add( impulse, rb->w, rb->w );
-#endif
+      v3f impulse;
+      v3_muls( delta,  lambda, impulse );
+      rb_linear_impulse( ra, wca, impulse );
+      v3_muls( delta, -lambda, impulse );
+      rb_linear_impulse( rb, wcb, impulse );
+
+      /* 'fake' snap */
+      v3_muladds( ra->co, delta,  dist * 0.01f, ra->co );
+      v3_muladds( rb->co, delta, -dist * 0.01f, rb->co );
+   }
 }
 
 static void debug_sphere( m4x3f m, float radius, u32 colour )