f6b9417a2b90e2fabb62b37d0d2fcc87cf5888fd
[vg.git] / 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 }