X-Git-Url: https://harrygodden.com/git/?p=vg.git;a=blobdiff_plain;f=vg_rigidbody.c;fp=vg_rigidbody.c;h=bcfebc5ba55c3dff32eae87592d83611997599bf;hp=0000000000000000000000000000000000000000;hb=3b14f3dcd5bf9dd3c85144f2123d667bfa4bb63f;hpb=fce86711735b15bff37de0f70716808410fcf269

diff --git a/vg_rigidbody.c b/vg_rigidbody.c
new file mode 100644
index 0000000..bcfebc5
--- /dev/null
+++ b/vg_rigidbody.c
@@ -0,0 +1,210 @@
+#include "vg_console.h"
+#include "vg_m.h"
+#include "vg_rigidbody.h"
+#include "vg_platform.h"
+#include "vg_engine.h"
+#include <math.h>
+
+static float
+   k_limit_bias       = 0.02f,
+   k_joint_correction = 0.01f,
+   k_joint_impulse    = 1.0f,
+   k_joint_bias       = 0.08f;               /* positional joints */
+
+static void rb_register_cvar(void)
+{
+   VG_VAR_F32( k_limit_bias, flags=VG_VAR_CHEAT );
+   VG_VAR_F32( k_joint_bias, flags=VG_VAR_CHEAT );
+   VG_VAR_F32( k_joint_correction, flags=VG_VAR_CHEAT );
+   VG_VAR_F32( k_joint_impulse, flags=VG_VAR_CHEAT );
+}
+
+void rb_setbody_capsule( rigidbody *rb, f32 r, f32 h, 
+                         f32 density, f32 inertia_scale ){
+   f32 vol  = vg_capsule_volume( r, h ),
+       mass = vol*density;
+
+   rb->inv_mass = 1.0f/mass;
+
+   m3x3f I;
+   vg_capsule_inertia( r, h, mass * inertia_scale, I );
+   m3x3_inv( I, rb->iI );
+}
+
+void rb_setbody_box( rigidbody *rb, boxf box, f32 density, f32 inertia_scale )
+{
+   f32 vol  = vg_box_volume( box ),
+       mass = vol*density;
+
+   rb->inv_mass = 1.0f/mass;
+   
+   m3x3f I;
+   vg_box_inertia( box, mass * inertia_scale, I );
+   m3x3_inv( I, rb->iI );
+}
+
+void rb_setbody_sphere( rigidbody *rb, f32 r, f32 density, f32 inertia_scale )
+{
+   f32 vol  = vg_sphere_volume( r ),
+       mass = vol*density;
+
+   rb->inv_mass = 1.0f/mass;
+   m3x3f I;
+   vg_sphere_inertia( r, mass * inertia_scale, I );
+   m3x3_inv( I, rb->iI );
+}
+
+void rb_update_matrices( rigidbody *rb )
+{
+   //q_normalize( rb->q );
+   q_m3x3( rb->q, rb->to_world );
+   v3_copy( rb->co, rb->to_world[3] );
+   m4x3_invert_affine( rb->to_world, rb->to_local );
+
+   /* I = R I_0 R^T */
+   m3x3_mul( rb->to_world, rb->iI, rb->iIw );
+   m3x3_mul( rb->iIw, rb->to_local, rb->iIw );
+}
+
+void rb_extrapolate( rigidbody *rb, v3f co, v4f q )
+{
+   float substep = vg.time_fixed_extrapolate;
+   v3_muladds( rb->co, rb->v, vg.time_fixed_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*vg.time_fixed_delta*substep );
+      q_mul( rotation, rb->q, q );
+      q_normalize( q );
+   }
+   else{
+      v4_copy( rb->q, q );
+   }
+}
+
+void rb_iter( rigidbody *rb )
+{
+   if( !vg_validf( rb->v[0] ) ||
+       !vg_validf( rb->v[1] ) ||
+       !vg_validf( rb->v[2] ) )
+   {
+      vg_fatal_error( "NaN velocity" );
+   }
+
+   v3f gravity = { 0.0f, -9.8f, 0.0f };
+   v3_muladds( rb->v, gravity, vg.time_fixed_delta, rb->v );
+
+   /* intergrate velocity */
+   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 ){
+      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*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.
+ */
+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 );
+}
+
+void rb_rcv( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb, v3f rv )
+{
+   v3f rva, rvb;
+   v3_cross( rba->w, ra, rva );
+   v3_add(   rba->v, rva, rva );
+   v3_cross( rbb->w, rb, rvb );
+   v3_add(   rbb->v, rvb, rvb );
+
+   v3_sub( rva, rvb, rv );
+}
+
+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 );
+}
+
+
+void rb_effect_simple_bouyency( rigidbody *ra, v4f plane, f32 amt, f32 drag )
+{
+   /* float */
+   float depth  = v3_dot( plane, ra->co ) - plane[3],
+         lambda = vg_clampf( -depth, 0.0f, 1.0f ) * amt;
+
+   v3_muladds( ra->v, plane, lambda * vg.time_fixed_delta, ra->v );
+
+   if( depth < 0.0f )
+      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 
+ * rt(worldspace)
+ */
+void rb_effect_spring_target_vector( rigidbody *rba, v3f ra, v3f rt,
+                                     f32 spring, f32 dampening, f32 timestep )
+{
+   float d = v3_dot( rt, ra );
+   float a = acosf( vg_clampf( d, -1.0f, 1.0f ) );
+
+   v3f axis;
+   v3_cross( rt, ra, axis );
+
+   float Fs = -a * spring,
+         Fd = -v3_dot( rba->w, axis ) * dampening;
+
+   v3_muladds( rba->w, axis, (Fs+Fd) * timestep, rba->w );
+}