vg_rigidbody.h

   1 #pragma once
   2
   3 /*
   4  * Copyright (C) 2021-2024 Mt.ZERO Software - All Rights Reserved
   5  *
   6  * Rigidbody main file, related:
   7  *  vg_rigidbody_collision.h
   8  *  vg_rigidbody_constraints.h
   9  */
  10
  11 #include "vg_console.h"
  12 #include <math.h>
  13
  14 /*
  15  * -----------------------------------------------------------------------------
  16  *                                (K)onstants
  17  * -----------------------------------------------------------------------------
  18  */
  19
  20 static const float
  21    //k_rb_rate          = (1.0/VG_TIMESTEP_FIXED),
  22    //k_rb_delta         = (1.0/k_rb_rate),
  23    k_friction         = 0.4f,
  24    k_damp_linear      = 0.1f,               /* scale velocity 1/(1+x) */
  25    k_damp_angular     = 0.1f,                /* scale angular  1/(1+x) */
  26    k_penetration_slop = 0.01f,
  27    k_inertia_scale    = 4.0f,
  28    k_phys_baumgarte   = 0.2f,
  29    k_gravity          = 9.6f,
  30    k_rb_density       = 8.0f;
  31
  32 static float
  33    k_limit_bias       = 0.02f,
  34    k_joint_correction = 0.01f,
  35    k_joint_impulse    = 1.0f,
  36    k_joint_bias       = 0.08f;               /* positional joints */
  37
  38 static void rb_register_cvar(void){
  39    VG_VAR_F32( k_limit_bias, flags=VG_VAR_CHEAT );
  40    VG_VAR_F32( k_joint_bias, flags=VG_VAR_CHEAT );
  41    VG_VAR_F32( k_joint_correction, flags=VG_VAR_CHEAT );
  42    VG_VAR_F32( k_joint_impulse, flags=VG_VAR_CHEAT );
  43 }
  44
  45 enum rb_shape {
  46    k_rb_shape_none    = 0,
  47    k_rb_shape_box     = 1,
  48    k_rb_shape_sphere  = 2,
  49    k_rb_shape_capsule = 3,
  50 };
  51
  52 /*
  53  * -----------------------------------------------------------------------------
  54  *                           structure definitions
  55  * -----------------------------------------------------------------------------
  56  */
  57
  58 typedef struct rigidbody rigidbody;
  59 typedef struct rb_capsule rb_capsule;
  60
  61 struct rb_capsule{
  62    f32 h, r;
  63 };
  64
  65 struct rigidbody{
  66    v3f co, v, w;
  67    v4f q;
  68
  69    f32 inv_mass;
  70
  71    m3x3f iI, iIw; /* inertia model and inverse world tensor */
  72    m4x3f to_world, to_local;
  73 };
  74
  75 /*
  76  * Initialize rigidbody inverse mass and inertia tensor with some common shapes
  77  */
  78 static void rb_setbody_capsule( rigidbody *rb, f32 r, f32 h,
  79                                 f32 density, f32 inertia_scale ){
  80    f32 vol  = vg_capsule_volume( r, h ),
  81        mass = vol*density;
  82
  83    rb->inv_mass = 1.0f/mass;
  84
  85    m3x3f I;
  86    vg_capsule_inertia( r, h, mass * inertia_scale, I );
  87    m3x3_inv( I, rb->iI );
  88 }
  89
  90 static void rb_setbody_box( rigidbody *rb, boxf box,
  91                             f32 density, f32 inertia_scale ){
  92    f32 vol  = vg_box_volume( box ),
  93        mass = vol*density;
  94
  95    rb->inv_mass = 1.0f/mass;
  96
  97    m3x3f I;
  98    vg_box_inertia( box, mass * inertia_scale, I );
  99    m3x3_inv( I, rb->iI );
 100 }
 101
 102 static void rb_setbody_sphere( rigidbody *rb, f32 r,
 103                                f32 density, f32 inertia_scale ){
 104    f32 vol  = vg_sphere_volume( r ),
 105        mass = vol*density;
 106
 107    rb->inv_mass = 1.0f/mass;
 108    m3x3f I;
 109    vg_sphere_inertia( r, mass * inertia_scale, I );
 110    m3x3_inv( I, rb->iI );
 111 }
 112
 113 /*
 114  * Update ALL matrices and tensors on rigidbody
 115  */
 116 static void rb_update_matrices( rigidbody *rb ){
 117    //q_normalize( rb->q );
 118    q_m3x3( rb->q, rb->to_world );
 119    v3_copy( rb->co, rb->to_world[3] );
 120    m4x3_invert_affine( rb->to_world, rb->to_local );
 121
 122    /* I = R I_0 R^T */
 123    m3x3_mul( rb->to_world, rb->iI, rb->iIw );
 124    m3x3_mul( rb->iIw, rb->to_local, rb->iIw );
 125 }
 126
 127 /*
 128  * Extrapolate rigidbody into a transform based on vg accumulator.
 129  * Useful for rendering
 130  */
 131 static void rb_extrapolate( rigidbody *rb, v3f co, v4f q ){
 132    float substep = vg.time_fixed_extrapolate;
 133    v3_muladds( rb->co, rb->v, vg.time_fixed_delta*substep, co );
 134
 135    if( v3_length2( rb->w ) > 0.0f ){
 136       v4f rotation;
 137       v3f axis;
 138       v3_copy( rb->w, axis );
 139
 140       float mag = v3_length( axis );
 141       v3_divs( axis, mag, axis );
 142       q_axis_angle( rotation, axis, mag*vg.time_fixed_delta*substep );
 143       q_mul( rotation, rb->q, q );
 144       q_normalize( q );
 145    }
 146    else{
 147       v4_copy( rb->q, q );
 148    }
 149 }
 150
 151 static void rb_iter( rigidbody *rb ){
 152    if( !vg_validf( rb->v[0] ) ||
 153        !vg_validf( rb->v[1] ) ||
 154        !vg_validf( rb->v[2] ) )
 155    {
 156       vg_fatal_error( "NaN velocity" );
 157    }
 158
 159    v3f gravity = { 0.0f, -9.8f, 0.0f };
 160    v3_muladds( rb->v, gravity, vg.time_fixed_delta, rb->v );
 161
 162    /* intergrate velocity */
 163    v3_muladds( rb->co, rb->v, vg.time_fixed_delta, rb->co );
 164 #if 0
 165    v3_lerp( rb->w, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->w );
 166 #endif
 167
 168    /* inegrate inertia */
 169    if( v3_length2( rb->w ) > 0.0f ){
 170       v4f rotation;
 171       v3f axis;
 172       v3_copy( rb->w, axis );
 173
 174       float mag = v3_length( axis );
 175       v3_divs( axis, mag, axis );
 176       q_axis_angle( rotation, axis, mag*vg.time_fixed_delta );
 177       q_mul( rotation, rb->q, rb->q );
 178       q_normalize( rb->q );
 179    }
 180 }
 181
 182 /*
 183  * based on: https://box2d.org/files/ErinCatto_NumericalMethods_GDC2015.pdf,
 184  * page 76.
 185  */
 186 static void rb_solve_gyroscopic( rigidbody *rb, m3x3f I, f32 h ){
 187    /* convert to body coordinates */
 188    v3f w_local;
 189    m3x3_mulv( rb->to_local, rb->w, w_local );
 190
 191    /* Residual vector */
 192    v3f f, v0;
 193    m3x3_mulv( I, w_local, v0 );
 194    v3_cross( w_local, v0, f );
 195    v3_muls( f, h, f );
 196
 197    /* Jacobian */
 198    m3x3f iJ, J, skew_w_local, skew_v0, m0;
 199    m3x3_skew_symetric( skew_w_local, w_local );
 200    m3x3_mul( skew_w_local, I, m0 );
 201
 202    m3x3_skew_symetric( skew_v0, v0 );
 203    m3x3_sub( m0, skew_v0, J );
 204    m3x3_scalef( J, h );
 205    m3x3_add( I, J, J );
 206
 207    /* Single Newton-Raphson update */
 208    v3f w1;
 209    m3x3_inv( J, iJ );
 210    m3x3_mulv( iJ, f, w1 );
 211    v3_sub( w_local, w1, rb->w );
 212
 213    m3x3_mulv( rb->to_world, rb->w, rb->w );
 214 }
 215
 216 /*
 217  * Creates relative contact velocity vector
 218  */
 219 static void rb_rcv( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb, v3f rv ){
 220    v3f rva, rvb;
 221    v3_cross( rba->w, ra, rva );
 222    v3_add(   rba->v, rva, rva );
 223    v3_cross( rbb->w, rb, rvb );
 224    v3_add(   rbb->v, rvb, rvb );
 225
 226    v3_sub( rva, rvb, rv );
 227 }
 228
 229 /*
 230  * Apply impulse to object
 231  */
 232 static void rb_linear_impulse( rigidbody *rb, v3f delta, v3f impulse ){
 233    /* linear */
 234    v3_muladds( rb->v, impulse,  rb->inv_mass, rb->v );
 235
 236    /* Angular velocity */
 237    v3f wa;
 238    v3_cross( delta, impulse, wa );
 239
 240    m3x3_mulv( rb->iIw, wa, wa );
 241    v3_add( rb->w, wa, rb->w );
 242 }
 243
 244 /*
 245  * Effectors
 246  */
 247
 248 static void rb_effect_simple_bouyency( rigidbody *ra, v4f plane,
 249                                           float amt, float drag ){
 250    /* float */
 251    float depth  = v3_dot( plane, ra->co ) - plane[3],
 252          lambda = vg_clampf( -depth, 0.0f, 1.0f ) * amt;
 253
 254    v3_muladds( ra->v, plane, lambda * vg.time_fixed_delta, ra->v );
 255
 256    if( depth < 0.0f )
 257       v3_muls( ra->v, 1.0f-(drag*vg.time_fixed_delta), ra->v );
 258 }
 259
 260 /* apply a spring&dampener force to match ra(worldspace) on rigidbody, to
 261  * rt(worldspace)
 262  */
 263 static void rb_effect_spring_target_vector( rigidbody *rba, v3f ra, v3f rt,
 264                                                float spring, float dampening,
 265                                                float timestep ){
 266    float d = v3_dot( rt, ra );
 267    float a = acosf( vg_clampf( d, -1.0f, 1.0f ) );
 268
 269    v3f axis;
 270    v3_cross( rt, ra, axis );
 271
 272    float Fs = -a * spring,
 273          Fd = -v3_dot( rba->w, axis ) * dampening;
 274
 275    v3_muladds( rba->w, axis, (Fs+Fd) * timestep, rba->w );
 276 }