vg_rigidbody.c

   1 #include "vg_console.h"
   2 #include "vg_m.h"
   3 #include "vg_rigidbody.h"
   4 #include "vg_platform.h"
   5 #include "vg_engine.h"
   6 #include <math.h>
   7
   8 static float
   9    k_limit_bias       = 0.02f,
  10    k_joint_correction = 0.01f,
  11    k_joint_impulse    = 1.0f,
  12    k_joint_bias       = 0.08f;               /* positional joints */
  13
  14 static void rb_register_cvar(void)
  15 {
  16    VG_VAR_F32( k_limit_bias, flags=VG_VAR_CHEAT );
  17    VG_VAR_F32( k_joint_bias, flags=VG_VAR_CHEAT );
  18    VG_VAR_F32( k_joint_correction, flags=VG_VAR_CHEAT );
  19    VG_VAR_F32( k_joint_impulse, flags=VG_VAR_CHEAT );
  20 }
  21
  22 void rb_setbody_capsule( rigidbody *rb, f32 r, f32 h,
  23                          f32 density, f32 inertia_scale ){
  24    f32 vol  = vg_capsule_volume( r, h ),
  25        mass = vol*density;
  26
  27    rb->inv_mass = 1.0f/mass;
  28
  29    m3x3f I;
  30    vg_capsule_inertia( r, h, mass * inertia_scale, I );
  31    m3x3_inv( I, rb->iI );
  32 }
  33
  34 void rb_setbody_box( rigidbody *rb, boxf box, f32 density, f32 inertia_scale )
  35 {
  36    f32 vol  = vg_box_volume( box ),
  37        mass = vol*density;
  38
  39    rb->inv_mass = 1.0f/mass;
  40
  41    m3x3f I;
  42    vg_box_inertia( box, mass * inertia_scale, I );
  43    m3x3_inv( I, rb->iI );
  44 }
  45
  46 void rb_setbody_sphere( rigidbody *rb, f32 r, f32 density, f32 inertia_scale )
  47 {
  48    f32 vol  = vg_sphere_volume( r ),
  49        mass = vol*density;
  50
  51    rb->inv_mass = 1.0f/mass;
  52    m3x3f I;
  53    vg_sphere_inertia( r, mass * inertia_scale, I );
  54    m3x3_inv( I, rb->iI );
  55 }
  56
  57 void rb_update_matrices( rigidbody *rb )
  58 {
  59    //q_normalize( rb->q );
  60    q_m3x3( rb->q, rb->to_world );
  61    v3_copy( rb->co, rb->to_world[3] );
  62    m4x3_invert_affine( rb->to_world, rb->to_local );
  63
  64    /* I = R I_0 R^T */
  65    m3x3_mul( rb->to_world, rb->iI, rb->iIw );
  66    m3x3_mul( rb->iIw, rb->to_local, rb->iIw );
  67 }
  68
  69 void rb_extrapolate( rigidbody *rb, v3f co, v4f q )
  70 {
  71    float substep = vg.time_fixed_extrapolate;
  72    v3_muladds( rb->co, rb->v, vg.time_fixed_delta*substep, co );
  73
  74    if( v3_length2( rb->w ) > 0.0f ){
  75       v4f rotation;
  76       v3f axis;
  77       v3_copy( rb->w, axis );
  78
  79       float mag = v3_length( axis );
  80       v3_divs( axis, mag, axis );
  81       q_axis_angle( rotation, axis, mag*vg.time_fixed_delta*substep );
  82       q_mul( rotation, rb->q, q );
  83       q_normalize( q );
  84    }
  85    else{
  86       v4_copy( rb->q, q );
  87    }
  88 }
  89
  90 void rb_iter( rigidbody *rb )
  91 {
  92    if( !vg_validf( rb->v[0] ) ||
  93        !vg_validf( rb->v[1] ) ||
  94        !vg_validf( rb->v[2] ) )
  95    {
  96       vg_fatal_error( "NaN velocity" );
  97    }
  98
  99    v3f gravity = { 0.0f, -9.8f, 0.0f };
 100    v3_muladds( rb->v, gravity, vg.time_fixed_delta, rb->v );
 101
 102    /* intergrate velocity */
 103    v3_muladds( rb->co, rb->v, vg.time_fixed_delta, rb->co );
 104 #if 0
 105    v3_lerp( rb->w, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->w );
 106 #endif
 107
 108    /* inegrate inertia */
 109    if( v3_length2( rb->w ) > 0.0f ){
 110       v4f rotation;
 111       v3f axis;
 112       v3_copy( rb->w, axis );
 113
 114       float mag = v3_length( axis );
 115       v3_divs( axis, mag, axis );
 116       q_axis_angle( rotation, axis, mag*vg.time_fixed_delta );
 117       q_mul( rotation, rb->q, rb->q );
 118       q_normalize( rb->q );
 119    }
 120 }
 121
 122 /*
 123  * based on: https://box2d.org/files/ErinCatto_NumericalMethods_GDC2015.pdf,
 124  * page 76.
 125  */
 126 void rb_solve_gyroscopic( rigidbody *rb, m3x3f I, f32 h )
 127 {
 128    /* convert to body coordinates */
 129    v3f w_local;
 130    m3x3_mulv( rb->to_local, rb->w, w_local );
 131
 132    /* Residual vector */
 133    v3f f, v0;
 134    m3x3_mulv( I, w_local, v0 );
 135    v3_cross( w_local, v0, f );
 136    v3_muls( f, h, f );
 137
 138    /* Jacobian */
 139    m3x3f iJ, J, skew_w_local, skew_v0, m0;
 140    m3x3_skew_symetric( skew_w_local, w_local );
 141    m3x3_mul( skew_w_local, I, m0 );
 142
 143    m3x3_skew_symetric( skew_v0, v0 );
 144    m3x3_sub( m0, skew_v0, J );
 145    m3x3_scalef( J, h );
 146    m3x3_add( I, J, J );
 147
 148    /* Single Newton-Raphson update */
 149    v3f w1;
 150    m3x3_inv( J, iJ );
 151    m3x3_mulv( iJ, f, w1 );
 152    v3_sub( w_local, w1, rb->w );
 153
 154    m3x3_mulv( rb->to_world, rb->w, rb->w );
 155 }
 156
 157 void rb_rcv( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb, v3f rv )
 158 {
 159    v3f rva, rvb;
 160    v3_cross( rba->w, ra, rva );
 161    v3_add(   rba->v, rva, rva );
 162    v3_cross( rbb->w, rb, rvb );
 163    v3_add(   rbb->v, rvb, rvb );
 164
 165    v3_sub( rva, rvb, rv );
 166 }
 167
 168 void rb_linear_impulse( rigidbody *rb, v3f delta, v3f impulse )
 169 {
 170    /* linear */
 171    v3_muladds( rb->v, impulse,  rb->inv_mass, rb->v );
 172
 173    /* Angular velocity */
 174    v3f wa;
 175    v3_cross( delta, impulse, wa );
 176
 177    m3x3_mulv( rb->iIw, wa, wa );
 178    v3_add( rb->w, wa, rb->w );
 179 }
 180
 181
 182 void rb_effect_simple_bouyency( rigidbody *ra, v4f plane, f32 amt, f32 drag )
 183 {
 184    /* float */
 185    float depth  = v3_dot( plane, ra->co ) - plane[3],
 186          lambda = vg_clampf( -depth, 0.0f, 1.0f ) * amt;
 187
 188    v3_muladds( ra->v, plane, lambda * vg.time_fixed_delta, ra->v );
 189
 190    if( depth < 0.0f )
 191       v3_muls( ra->v, 1.0f-(drag*vg.time_fixed_delta), ra->v );
 192 }
 193
 194 /* apply a spring&dampener force to match ra(worldspace) on rigidbody, to
 195  * rt(worldspace)
 196  */
 197 void rb_effect_spring_target_vector( rigidbody *rba, v3f ra, v3f rt,
 198                                      f32 spring, f32 dampening, f32 timestep )
 199 {
 200    float d = v3_dot( rt, ra );
 201    float a = acosf( vg_clampf( d, -1.0f, 1.0f ) );
 202
 203    v3f axis;
 204    v3_cross( rt, ra, axis );
 205
 206    float Fs = -a * spring,
 207          Fd = -v3_dot( rba->w, axis ) * dampening;
 208
 209    v3_muladds( rba->w, axis, (Fs+Fd) * timestep, rba->w );
 210 }