#pragma once
-
/*
* Copyright (C) 2021-2024 Mt.ZERO Software - All Rights Reserved
*
* vg_rigidbody_constraints.h
*/
-#include "vg_console.h"
-#include <math.h>
-
-/*
- * -----------------------------------------------------------------------------
- * (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,
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;
/*
* 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 );
projects / vg.git / blobdiff