labs work

[vg.git] / vg_rigidbody.h

diff --git a/vg_rigidbody.h b/vg_rigidbody.h
index c44aa5d9e649b2504b8f266f7357e0a2bf630ebf..f6b9417a2b90e2fabb62b37d0d2fcc87cf5888fd 100644 (file)
--- a/vg_rigidbody.h
+++ b/vg_rigidbody.h
@@ -18,8 +18,8 @@
  */
 
 static const float 
-   k_rb_rate          = (1.0/VG_TIMESTEP_FIXED),
-   k_rb_delta         = (1.0/k_rb_rate),
+   //k_rb_rate          = (1.0/VG_TIMESTEP_FIXED),
+   //k_rb_delta         = (1.0/k_rb_rate),
    k_friction         = 0.4f,
    k_damp_linear      = 0.1f,               /* scale velocity 1/(1+x) */
    k_damp_angular     = 0.1f,                /* scale angular  1/(1+x) */
@@ -130,7 +130,7 @@ static void rb_update_matrices( rigidbody *rb ){
  */
 static void rb_extrapolate( rigidbody *rb, v3f co, v4f q ){
    float substep = vg.time_fixed_extrapolate;
-   v3_muladds( rb->co, rb->v, k_rb_delta*substep, co );
+   v3_muladds( rb->co, rb->v, vg.time_fixed_delta*substep, co );
 
    if( v3_length2( rb->w ) > 0.0f ){
       v4f rotation;
@@ -139,7 +139,7 @@ static void rb_extrapolate( rigidbody *rb, v3f co, v4f q ){
       
       float mag = v3_length( axis );
       v3_divs( axis, mag, axis );
-      q_axis_angle( rotation, axis, mag*k_rb_delta*substep );
+      q_axis_angle( rotation, axis, mag*vg.time_fixed_delta*substep );
       q_mul( rotation, rb->q, q );
       q_normalize( q );
    }
@@ -157,11 +157,13 @@ static void rb_iter( rigidbody *rb ){
    }
 
    v3f gravity = { 0.0f, -9.8f, 0.0f };
-   v3_muladds( rb->v, gravity, k_rb_delta, rb->v );
+   v3_muladds( rb->v, gravity, vg.time_fixed_delta, rb->v );
 
    /* intergrate velocity */
-   v3_muladds( rb->co, rb->v, k_rb_delta, rb->co );
+   v3_muladds( rb->co, rb->v, vg.time_fixed_delta, rb->co );
+#if 0
    v3_lerp( rb->w, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->w );
+#endif
 
    /* inegrate inertia */
    if( v3_length2( rb->w ) > 0.0f ){
@@ -171,12 +173,46 @@ static void rb_iter( rigidbody *rb ){
       
       float mag = v3_length( axis );
       v3_divs( axis, mag, axis );
-      q_axis_angle( rotation, axis, mag*k_rb_delta );
+      q_axis_angle( rotation, axis, mag*vg.time_fixed_delta );
       q_mul( rotation, rb->q, rb->q );
       q_normalize( rb->q );
    }
 }
 
+/* 
+ * based on: https://box2d.org/files/ErinCatto_NumericalMethods_GDC2015.pdf,
+ * page 76.
+ */
+static void rb_solve_gyroscopic( rigidbody *rb, m3x3f I, f32 h ){
+   /* convert to body coordinates */
+   v3f w_local;
+   m3x3_mulv( rb->to_local, rb->w, w_local );
+
+   /* Residual vector */
+   v3f f, v0;
+   m3x3_mulv( I, w_local, v0 );
+   v3_cross( w_local, v0, f );
+   v3_muls( f, h, f );
+
+   /* Jacobian */
+   m3x3f iJ, J, skew_w_local, skew_v0, m0;
+   m3x3_skew_symetric( skew_w_local, w_local );
+   m3x3_mul( skew_w_local, I, m0 );
+
+   m3x3_skew_symetric( skew_v0, v0 );
+   m3x3_sub( m0, skew_v0, J );
+   m3x3_scalef( J, h );
+   m3x3_add( I, J, J );
+
+   /* Single Newton-Raphson update */
+   v3f w1;
+   m3x3_inv( J, iJ );
+   m3x3_mulv( iJ, f, w1 );
+   v3_sub( w_local, w1, rb->w );
+
+   m3x3_mulv( rb->to_world, rb->w, rb->w );
+}
+
 /*
  * Creates relative contact velocity vector
  */
@@ -215,10 +251,10 @@ static void rb_effect_simple_bouyency( rigidbody *ra, v4f plane,
    float depth  = v3_dot( plane, ra->co ) - plane[3],
          lambda = vg_clampf( -depth, 0.0f, 1.0f ) * amt;
 
-   v3_muladds( ra->v, plane, lambda * k_rb_delta, ra->v );
+   v3_muladds( ra->v, plane, lambda * vg.time_fixed_delta, ra->v );
 
    if( depth < 0.0f )
-      v3_muls( ra->v, 1.0f-(drag*k_rb_delta), ra->v );
+      v3_muls( ra->v, 1.0f-(drag*vg.time_fixed_delta), ra->v );
 }
 
 /* apply a spring&dampener force to match ra(worldspace) on rigidbody, to