1 #include "vg_console.h"
3 #include "vg_rigidbody.h"
4 #include "vg_platform.h"
10 k_joint_correction
= 0.01f
,
11 k_joint_impulse
= 1.0f
,
12 k_joint_bias
= 0.08f
; /* positional joints */
14 static void rb_register_cvar(void)
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
);
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
),
27 rb
->inv_mass
= 1.0f
/mass
;
30 vg_capsule_inertia( r
, h
, mass
* inertia_scale
, I
);
31 m3x3_inv( I
, rb
->iI
);
34 void rb_setbody_box( rigidbody
*rb
, boxf box
, f32 density
, f32 inertia_scale
)
36 f32 vol
= vg_box_volume( box
),
39 rb
->inv_mass
= 1.0f
/mass
;
42 vg_box_inertia( box
, mass
* inertia_scale
, I
);
43 m3x3_inv( I
, rb
->iI
);
46 void rb_setbody_sphere( rigidbody
*rb
, f32 r
, f32 density
, f32 inertia_scale
)
48 f32 vol
= vg_sphere_volume( r
),
51 rb
->inv_mass
= 1.0f
/mass
;
53 vg_sphere_inertia( r
, mass
* inertia_scale
, I
);
54 m3x3_inv( I
, rb
->iI
);
57 void rb_update_matrices( rigidbody
*rb
)
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
);
65 m3x3_mul( rb
->to_world
, rb
->iI
, rb
->iIw
);
66 m3x3_mul( rb
->iIw
, rb
->to_local
, rb
->iIw
);
69 void rb_extrapolate( rigidbody
*rb
, v3f co
, v4f q
)
71 float substep
= vg
.time_fixed_extrapolate
;
72 v3_muladds( rb
->co
, rb
->v
, vg
.time_fixed_delta
*substep
, co
);
74 if( v3_length2( rb
->w
) > 0.0f
){
77 v3_copy( rb
->w
, axis
);
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
);
90 void rb_iter( rigidbody
*rb
)
92 if( !vg_validf( rb
->v
[0] ) ||
93 !vg_validf( rb
->v
[1] ) ||
94 !vg_validf( rb
->v
[2] ) )
96 vg_fatal_error( "NaN velocity" );
99 v3f gravity
= { 0.0f
, -9.8f
, 0.0f
};
100 v3_muladds( rb
->v
, gravity
, vg
.time_fixed_delta
, rb
->v
);
102 /* intergrate velocity */
103 v3_muladds( rb
->co
, rb
->v
, vg
.time_fixed_delta
, rb
->co
);
105 v3_lerp( rb
->w
, (v3f
){0.0f
,0.0f
,0.0f
}, 0.0025f
, rb
->w
);
108 /* inegrate inertia */
109 if( v3_length2( rb
->w
) > 0.0f
){
112 v3_copy( rb
->w
, axis
);
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
);
123 * based on: https://box2d.org/files/ErinCatto_NumericalMethods_GDC2015.pdf,
126 void rb_solve_gyroscopic( rigidbody
*rb
, m3x3f I
, f32 h
)
128 /* convert to body coordinates */
130 m3x3_mulv( rb
->to_local
, rb
->w
, w_local
);
132 /* Residual vector */
134 m3x3_mulv( I
, w_local
, v0
);
135 v3_cross( w_local
, v0
, f
);
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
);
143 m3x3_skew_symetric( skew_v0
, v0
);
144 m3x3_sub( m0
, skew_v0
, J
);
148 /* Single Newton-Raphson update */
151 m3x3_mulv( iJ
, f
, w1
);
152 v3_sub( w_local
, w1
, rb
->w
);
154 m3x3_mulv( rb
->to_world
, rb
->w
, rb
->w
);
157 void rb_rcv( rigidbody
*rba
, rigidbody
*rbb
, v3f ra
, v3f rb
, v3f rv
)
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
);
165 v3_sub( rva
, rvb
, rv
);
168 void rb_linear_impulse( rigidbody
*rb
, v3f delta
, v3f impulse
)
171 v3_muladds( rb
->v
, impulse
, rb
->inv_mass
, rb
->v
);
173 /* Angular velocity */
175 v3_cross( delta
, impulse
, wa
);
177 m3x3_mulv( rb
->iIw
, wa
, wa
);
178 v3_add( rb
->w
, wa
, rb
->w
);
182 void rb_effect_simple_bouyency( rigidbody
*ra
, v4f plane
, f32 amt
, f32 drag
)
185 float depth
= v3_dot( plane
, ra
->co
) - plane
[3],
186 lambda
= vg_clampf( -depth
, 0.0f
, 1.0f
) * amt
;
188 v3_muladds( ra
->v
, plane
, lambda
* vg
.time_fixed_delta
, ra
->v
);
191 v3_muls( ra
->v
, 1.0f
-(drag
*vg
.time_fixed_delta
), ra
->v
);
194 /* apply a spring&dampener force to match ra(worldspace) on rigidbody, to
197 void rb_effect_spring_target_vector( rigidbody
*rba
, v3f ra
, v3f rt
,
198 f32 spring
, f32 dampening
, f32 timestep
)
200 float d
= v3_dot( rt
, ra
);
201 float a
= acosf( vg_clampf( d
, -1.0f
, 1.0f
) );
204 v3_cross( rt
, ra
, axis
);
206 float Fs
= -a
* spring
,
207 Fd
= -v3_dot( rba
->w
, axis
) * dampening
;
209 v3_muladds( rba
->w
, axis
, (Fs
+Fd
) * timestep
, rba
->w
);