X-Git-Url: https://harrygodden.com/git/?p=vg.git;a=blobdiff_plain;f=vg_rigidbody.h;fp=vg_rigidbody.h;h=70be483dbe8371c64b691970e6c33ede9d46b495;hp=f6b9417a2b90e2fabb62b37d0d2fcc87cf5888fd;hb=3b14f3dcd5bf9dd3c85144f2123d667bfa4bb63f;hpb=fce86711735b15bff37de0f70716808410fcf269 diff --git a/vg_rigidbody.h b/vg_rigidbody.h index f6b9417..70be483 100644 --- a/vg_rigidbody.h +++ b/vg_rigidbody.h @@ -1,5 +1,4 @@ #pragma once - /* * Copyright (C) 2021-2024 Mt.ZERO Software - All Rights Reserved * @@ -8,39 +7,14 @@ * vg_rigidbody_constraints.h */ -#include "vg_console.h" -#include - -/* - * ----------------------------------------------------------------------------- - * (K)onstants - * ----------------------------------------------------------------------------- - */ - -static const float - //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) */ - k_penetration_slop = 0.01f, - k_inertia_scale = 4.0f, - k_phys_baumgarte = 0.2f, - k_gravity = 9.6f, - k_rb_density = 8.0f; - -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 ); -} +#define k_friction 0.4f +#define k_damp_linear 0.1f /* scale velocity 1/(1+x) */ +#define k_damp_angular 0.1f /* scale angular 1/(1+x) */ +#define k_penetration_slop 0.01f +#define k_rb_inertia_scale 4.0f +#define k_phys_baumgarte 0.2f +#define k_gravity 9.6f +#define k_rb_density 8.0f enum rb_shape { k_rb_shape_none = 0, @@ -49,20 +23,16 @@ enum rb_shape { k_rb_shape_capsule = 3, }; -/* - * ----------------------------------------------------------------------------- - * structure definitions - * ----------------------------------------------------------------------------- - */ - typedef struct rigidbody rigidbody; typedef struct rb_capsule rb_capsule; -struct rb_capsule{ +struct rb_capsule +{ f32 h, r; }; -struct rigidbody{ +struct rigidbody +{ v3f co, v, w; v4f q; @@ -75,202 +45,37 @@ struct rigidbody{ /* * Initialize rigidbody inverse mass and inertia tensor with some common shapes */ -static 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 ); -} - -static 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 ); -} - -static 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_setbody_capsule( rigidbody *rb, f32 r, f32 h, + f32 density, f32 inertia_scale ); +void rb_setbody_box( rigidbody *rb, boxf box, f32 density, f32 inertia_scale ); +void rb_setbody_sphere( rigidbody *rb, f32 r, f32 density, f32 inertia_scale ); /* * Update ALL matrices and tensors on rigidbody */ -static 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_update_matrices( rigidbody *rb ); /* * Extrapolate rigidbody into a transform based on vg accumulator. * Useful for rendering */ -static 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 ); - } -} - -static 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. - */ -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 ); -} +void rb_extrapolate( rigidbody *rb, v3f co, v4f q ); +void rb_iter( rigidbody *rb ); +void rb_solve_gyroscopic( rigidbody *rb, m3x3f I, f32 h ); /* * Creates relative contact velocity vector */ -static 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_rcv( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb, v3f rv ); /* * Apply impulse to object */ -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 ); -} +void rb_linear_impulse( rigidbody *rb, v3f delta, v3f impulse ); /* * Effectors */ - -static void rb_effect_simple_bouyency( rigidbody *ra, v4f plane, - float amt, float 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) - */ -static void rb_effect_spring_target_vector( rigidbody *rba, v3f ra, v3f rt, - float spring, float dampening, - float 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 ); -} +void rb_effect_simple_bouyency( rigidbody *ra, v4f plane, f32 amt, f32 drag ); +void rb_effect_spring_target_vector( rigidbody *rba, v3f ra, v3f rt, + f32 spring, f32 dampening, f32 timestep );