2 * Copyright (C) 2021-2023 Mt.ZERO Software, Harry Godden - All Rights Reserved
6 * Resources: Box2D - Erin Catto
10 #include "vg/vg_console.h"
16 static bh_system bh_system_rigidbodies
;
22 * -----------------------------------------------------------------------------
24 * -----------------------------------------------------------------------------
28 k_rb_rate
= (1.0/VG_TIMESTEP_FIXED
),
29 k_rb_delta
= (1.0/k_rb_rate
),
31 k_damp_linear
= 0.1f
, /* scale velocity 1/(1+x) */
32 k_damp_angular
= 0.1f
, /* scale angular 1/(1+x) */
33 k_penetration_slop
= 0.01f
,
34 k_inertia_scale
= 8.0f
,
35 k_phys_baumgarte
= 0.2f
,
40 k_joint_correction
= 0.01f
,
41 k_joint_impulse
= 1.0f
,
42 k_joint_bias
= 0.08f
; /* positional joints */
44 static void rb_register_cvar(void){
45 VG_VAR_F32( k_limit_bias
, flags
=VG_VAR_CHEAT
);
46 VG_VAR_F32( k_joint_bias
, flags
=VG_VAR_CHEAT
);
47 VG_VAR_F32( k_joint_correction
, flags
=VG_VAR_CHEAT
);
48 VG_VAR_F32( k_joint_impulse
, flags
=VG_VAR_CHEAT
);
52 * -----------------------------------------------------------------------------
53 * structure definitions
54 * -----------------------------------------------------------------------------
57 typedef struct rigidbody rigidbody
;
58 typedef struct rb_object rb_object
;
59 typedef struct contact rb_ct
;
60 typedef struct rb_sphere rb_sphere
;
61 typedef struct rb_capsule rb_capsule
;
62 typedef struct rb_scene rb_scene
;
83 /* inertia model and inverse world tensor */
86 m4x3f to_world
, to_local
;
94 k_rb_shape_sphere
= 1,
95 k_rb_shape_capsule
= 2,
101 struct rb_sphere sphere
;
102 struct rb_capsule capsule
;
103 struct rb_scene scene
;
108 static struct contact
{
109 rigidbody
*rba
, *rbb
;
112 float p
, bias
, norm_impulse
, tangent_impulse
[2],
113 normal_mass
, tangent_mass
[2];
117 enum contact_type type
;
119 rb_contact_buffer
[256];
120 static int rb_contact_count
= 0;
122 typedef struct rb_constr_pos rb_constr_pos
;
123 typedef struct rb_constr_swingtwist rb_constr_swingtwist
;
125 struct rb_constr_pos
{
126 rigidbody
*rba
, *rbb
;
130 struct rb_constr_swingtwist
{
131 rigidbody
*rba
, *rbb
;
133 v4f conevx
, conevy
; /* relative to rba */
134 v3f view_offset
, /* relative to rba */
135 coneva
, conevxb
;/* relative to rbb */
137 int tangent_violation
, axis_violation
;
138 v3f axis
, tangent_axis
, tangent_target
, axis_target
;
141 float tangent_mass
, axis_mass
;
145 * -----------------------------------------------------------------------------
147 * -----------------------------------------------------------------------------
150 static void rb_debug_contact( rb_ct
*ct
){
152 v3_muladds( ct
->co
, ct
->n
, 0.05f
, p1
);
154 if( ct
->type
== k_contact_type_default
){
155 vg_line_point( ct
->co
, 0.0125f
, 0xff0000ff );
156 vg_line( ct
->co
, p1
, 0xffffffff );
158 else if( ct
->type
== k_contact_type_edge
){
159 vg_line_point( ct
->co
, 0.0125f
, 0xff00ffc0 );
160 vg_line( ct
->co
, p1
, 0xffffffff );
165 static void rb_object_debug( rb_object
*obj
, u32 colour
){
166 if( obj
->type
== k_rb_shape_box
){
167 v3f
*box
= obj
->rb
.bbx
;
168 vg_line_boxf_transformed( obj
->rb
.to_world
, obj
->rb
.bbx
, colour
);
170 else if( obj
->type
== k_rb_shape_sphere
){
171 vg_line_sphere( obj
->rb
.to_world
, obj
->inf
.sphere
.radius
, colour
);
173 else if( obj
->type
== k_rb_shape_capsule
){
175 float h
= obj
->inf
.capsule
.height
,
176 r
= obj
->inf
.capsule
.radius
;
178 vg_line_capsule( obj
->rb
.to_world
, r
, h
, colour
);
180 else if( obj
->type
== k_rb_shape_scene
){
181 vg_line_boxf( obj
->rb
.bbx
, colour
);
186 * -----------------------------------------------------------------------------
188 * -----------------------------------------------------------------------------
192 * Update world space bounding box based on local one
194 static void rb_update_bounds( rigidbody
*rb
){
195 box_init_inf( rb
->bbx_world
);
196 m4x3_expand_aabb_aabb( rb
->to_world
, rb
->bbx_world
, rb
->bbx
);
200 * Commit transform to rigidbody. Updates matrices
202 static void rb_update_transform( rigidbody
*rb
)
204 q_normalize( rb
->q
);
205 q_m3x3( rb
->q
, rb
->to_world
);
206 v3_copy( rb
->co
, rb
->to_world
[3] );
208 m4x3_invert_affine( rb
->to_world
, rb
->to_local
);
209 m3x3_mul( rb
->iI
, rb
->to_local
, rb
->iIw
);
210 m3x3_mul( rb
->to_world
, rb
->iIw
, rb
->iIw
);
212 rb_update_bounds( rb
);
216 * Extrapolate rigidbody into a transform based on vg accumulator.
217 * Useful for rendering
219 static void rb_extrapolate( rigidbody
*rb
, v3f co
, v4f q
)
221 float substep
= vg
.time_fixed_extrapolate
;
222 v3_muladds( rb
->co
, rb
->v
, k_rb_delta
*substep
, co
);
224 if( v3_length2( rb
->w
) > 0.0f
){
227 v3_copy( rb
->w
, axis
);
229 float mag
= v3_length( axis
);
230 v3_divs( axis
, mag
, axis
);
231 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
*substep
);
232 q_mul( rotation
, rb
->q
, q
);
241 * Initialize rigidbody and calculate masses, inertia
243 static void rb_init_object( rb_object
*obj
){
247 if( obj
->type
== k_rb_shape_box
){
249 v3_sub( obj
->rb
.bbx
[1], obj
->rb
.bbx
[0], dims
);
250 volume
= dims
[0]*dims
[1]*dims
[2];
252 else if( obj
->type
== k_rb_shape_sphere
){
253 volume
= vg_sphere_volume( obj
->inf
.sphere
.radius
);
254 v3_fill( obj
->rb
.bbx
[0], -obj
->inf
.sphere
.radius
);
255 v3_fill( obj
->rb
.bbx
[1], obj
->inf
.sphere
.radius
);
257 else if( obj
->type
== k_rb_shape_capsule
){
258 float r
= obj
->inf
.capsule
.radius
,
259 h
= obj
->inf
.capsule
.height
;
260 volume
= vg_sphere_volume( r
) + VG_PIf
* r
*r
* (h
- r
*2.0f
);
262 v3_fill( obj
->rb
.bbx
[0], -r
);
263 v3_fill( obj
->rb
.bbx
[1], r
);
264 obj
->rb
.bbx
[0][1] = -h
;
265 obj
->rb
.bbx
[1][1] = h
;
267 else if( obj
->type
== k_rb_shape_scene
){
269 box_copy( obj
->inf
.scene
.bh_scene
->nodes
[0].bbx
, obj
->rb
.bbx
);
273 obj
->rb
.inv_mass
= 0.0f
;
274 v3_zero( obj
->rb
.I
);
275 m3x3_zero( obj
->rb
.iI
);
278 float mass
= 2.0f
*volume
;
279 obj
->rb
.inv_mass
= 1.0f
/mass
;
282 v3_sub( obj
->rb
.bbx
[1], obj
->rb
.bbx
[0], extent
);
283 v3_muls( extent
, 0.5f
, extent
);
285 /* local intertia tensor */
286 float scale
= k_inertia_scale
;
287 float ex2
= scale
*extent
[0]*extent
[0],
288 ey2
= scale
*extent
[1]*extent
[1],
289 ez2
= scale
*extent
[2]*extent
[2];
291 obj
->rb
.I
[0] = ((1.0f
/12.0f
) * mass
* (ey2
+ez2
));
292 obj
->rb
.I
[1] = ((1.0f
/12.0f
) * mass
* (ex2
+ez2
));
293 obj
->rb
.I
[2] = ((1.0f
/12.0f
) * mass
* (ex2
+ey2
));
295 m3x3_identity( obj
->rb
.iI
);
296 obj
->rb
.iI
[0][0] = obj
->rb
.I
[0];
297 obj
->rb
.iI
[1][1] = obj
->rb
.I
[1];
298 obj
->rb
.iI
[2][2] = obj
->rb
.I
[2];
299 m3x3_inv( obj
->rb
.iI
, obj
->rb
.iI
);
302 rb_update_transform( &obj
->rb
);
305 static void rb_iter( rigidbody
*rb
){
306 if( !vg_validf( rb
->v
[0] ) ||
307 !vg_validf( rb
->v
[1] ) ||
308 !vg_validf( rb
->v
[2] ) )
310 vg_fatal_error( "NaN velocity" );
313 v3f gravity
= { 0.0f
, -9.8f
, 0.0f
};
314 v3_muladds( rb
->v
, gravity
, k_rb_delta
, rb
->v
);
316 /* intergrate velocity */
317 v3_muladds( rb
->co
, rb
->v
, k_rb_delta
, rb
->co
);
318 v3_lerp( rb
->w
, (v3f
){0.0f
,0.0f
,0.0f
}, 0.0025f
, rb
->w
);
320 /* inegrate inertia */
321 if( v3_length2( rb
->w
) > 0.0f
)
325 v3_copy( rb
->w
, axis
);
327 float mag
= v3_length( axis
);
328 v3_divs( axis
, mag
, axis
);
329 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
);
330 q_mul( rotation
, rb
->q
, rb
->q
);
335 v3_muls( rb
->v
, 1.0f
/(1.0f
+k_rb_delta
*k_damp_linear
), rb
->v
);
336 v3_muls( rb
->w
, 1.0f
/(1.0f
+k_rb_delta
*k_damp_angular
), rb
->w
);
342 * -----------------------------------------------------------------------------
343 * Boolean shape overlap functions
344 * -----------------------------------------------------------------------------
348 * Project AABB, and triangle interval onto axis to check if they overlap
350 static int rb_box_triangle_interval( v3f extent
, v3f axis
, v3f tri
[3] ){
353 r
= extent
[0] * fabsf(axis
[0]) +
354 extent
[1] * fabsf(axis
[1]) +
355 extent
[2] * fabsf(axis
[2]),
357 p0
= v3_dot( axis
, tri
[0] ),
358 p1
= v3_dot( axis
, tri
[1] ),
359 p2
= v3_dot( axis
, tri
[2] ),
361 e
= vg_maxf(-vg_maxf(p0
,vg_maxf(p1
,p2
)), vg_minf(p0
,vg_minf(p1
,p2
)));
363 if( e
> r
) return 0;
368 * Seperating axis test box vs triangle
370 static int rb_box_triangle_sat( v3f extent
, v3f center
,
371 m4x3f to_local
, v3f tri_src
[3] ){
374 for( int i
=0; i
<3; i
++ ){
375 m4x3_mulv( to_local
, tri_src
[i
], tri
[i
] );
376 v3_sub( tri
[i
], center
, tri
[i
] );
380 v3_sub( tri
[1], tri
[0], f0
);
381 v3_sub( tri
[2], tri
[1], f1
);
382 v3_sub( tri
[0], tri
[2], f2
);
386 v3_cross( (v3f
){1.0f
,0.0f
,0.0f
}, f0
, axis
[0] );
387 v3_cross( (v3f
){1.0f
,0.0f
,0.0f
}, f1
, axis
[1] );
388 v3_cross( (v3f
){1.0f
,0.0f
,0.0f
}, f2
, axis
[2] );
389 v3_cross( (v3f
){0.0f
,1.0f
,0.0f
}, f0
, axis
[3] );
390 v3_cross( (v3f
){0.0f
,1.0f
,0.0f
}, f1
, axis
[4] );
391 v3_cross( (v3f
){0.0f
,1.0f
,0.0f
}, f2
, axis
[5] );
392 v3_cross( (v3f
){0.0f
,0.0f
,1.0f
}, f0
, axis
[6] );
393 v3_cross( (v3f
){0.0f
,0.0f
,1.0f
}, f1
, axis
[7] );
394 v3_cross( (v3f
){0.0f
,0.0f
,1.0f
}, f2
, axis
[8] );
396 for( int i
=0; i
<9; i
++ )
397 if(!rb_box_triangle_interval( extent
, axis
[i
], tri
)) return 0;
400 if(!rb_box_triangle_interval( extent
, (v3f
){1.0f
,0.0f
,0.0f
}, tri
)) return 0;
401 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,1.0f
,0.0f
}, tri
)) return 0;
402 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,0.0f
,1.0f
}, tri
)) return 0;
405 v3_cross( f0
, f1
, n
);
406 if(!rb_box_triangle_interval( extent
, n
, tri
)) return 0;
412 * -----------------------------------------------------------------------------
414 * -----------------------------------------------------------------------------
417 static int rb_manifold_apply_filtered( rb_ct
*man
, int len
){
420 for( int i
=0; i
<len
; i
++ ){
423 if( ct
->type
== k_contact_type_disabled
)
433 * Merge two contacts if they are within radius(r) of eachother
435 static void rb_manifold_contact_weld( rb_ct
*ci
, rb_ct
*cj
, float r
){
436 if( v3_dist2( ci
->co
, cj
->co
) < r
*r
){
437 cj
->type
= k_contact_type_disabled
;
438 ci
->p
= (ci
->p
+ cj
->p
) * 0.5f
;
440 v3_add( ci
->co
, cj
->co
, ci
->co
);
441 v3_muls( ci
->co
, 0.5f
, ci
->co
);
444 v3_sub( ci
->rba
->co
, ci
->co
, delta
);
446 float c0
= v3_dot( ci
->n
, delta
),
447 c1
= v3_dot( cj
->n
, delta
);
449 if( c0
< 0.0f
|| c1
< 0.0f
){
451 ci
->type
= k_contact_type_disabled
;
455 v3_muls( ci
->n
, c0
, n
);
456 v3_muladds( n
, cj
->n
, c1
, n
);
466 static void rb_manifold_filter_joint_edges( rb_ct
*man
, int len
, float r
){
467 for( int i
=0; i
<len
-1; i
++ ){
469 if( ci
->type
!= k_contact_type_edge
)
472 for( int j
=i
+1; j
<len
; j
++ ){
474 if( cj
->type
!= k_contact_type_edge
)
477 rb_manifold_contact_weld( ci
, cj
, r
);
483 * Resolve overlapping pairs
485 static void rb_manifold_filter_pairs( rb_ct
*man
, int len
, float r
){
486 for( int i
=0; i
<len
-1; i
++ ){
490 if( ci
->type
== k_contact_type_disabled
) continue;
492 for( int j
=i
+1; j
<len
; j
++ ){
495 if( cj
->type
== k_contact_type_disabled
) continue;
497 if( v3_dist2( ci
->co
, cj
->co
) < r
*r
){
498 cj
->type
= k_contact_type_disabled
;
499 v3_add( cj
->n
, ci
->n
, ci
->n
);
506 float n
= 1.0f
/((float)similar
+1.0f
);
507 v3_muls( ci
->n
, n
, ci
->n
);
510 if( v3_length2(ci
->n
) < 0.1f
*0.1f
)
511 ci
->type
= k_contact_type_disabled
;
513 v3_normalize( ci
->n
);
519 * Remove contacts that are facing away from A
521 static void rb_manifold_filter_backface( rb_ct
*man
, int len
){
522 for( int i
=0; i
<len
; i
++ ){
524 if( ct
->type
== k_contact_type_disabled
)
528 v3_sub( ct
->co
, ct
->rba
->co
, delta
);
530 if( v3_dot( delta
, ct
->n
) > -0.001f
)
531 ct
->type
= k_contact_type_disabled
;
536 * Filter out duplicate coplanar results. Good for spheres.
538 static void rb_manifold_filter_coplanar( rb_ct
*man
, int len
, float w
){
539 for( int i
=0; i
<len
; i
++ ){
541 if( ci
->type
== k_contact_type_disabled
||
542 ci
->type
== k_contact_type_edge
)
545 float d1
= v3_dot( ci
->co
, ci
->n
);
547 for( int j
=0; j
<len
; j
++ ){
552 if( cj
->type
== k_contact_type_disabled
)
555 float d2
= v3_dot( cj
->co
, ci
->n
),
558 if( fabsf( d
) <= w
){
559 cj
->type
= k_contact_type_disabled
;
566 * -----------------------------------------------------------------------------
568 * -----------------------------------------------------------------------------
574 * These do not automatically allocate contacts, an appropriately sized
575 * buffer must be supplied. The function returns the size of the manifold
576 * which was generated.
578 * The values set on the contacts are: n, co, p, rba, rbb
582 * By collecting the minimum(time) and maximum(time) pairs of points, we
583 * build a reduced and stable exact manifold.
586 * rx: minimum distance of these points
587 * dx: the delta between the two points
589 * pairs will only ammend these if they are creating a collision
591 typedef struct capsule_manifold capsule_manifold
;
592 struct capsule_manifold
{
599 * Expand a line manifold with a new pair. t value is the time along segment
600 * on the oriented object which created this pair.
602 static void rb_capsule_manifold( v3f pa
, v3f pb
, float t
, float r
,
603 capsule_manifold
*manifold
){
605 v3_sub( pa
, pb
, delta
);
607 if( v3_length2(delta
) < r
*r
){
608 if( t
< manifold
->t0
){
609 v3_copy( delta
, manifold
->d0
);
614 if( t
> manifold
->t1
){
615 v3_copy( delta
, manifold
->d1
);
622 static void rb_capsule_manifold_init( capsule_manifold
*manifold
){
623 manifold
->t0
= INFINITY
;
624 manifold
->t1
= -INFINITY
;
627 static int rb_capsule__manifold_done( m4x3f mtx
, rb_capsule
*c
,
628 capsule_manifold
*manifold
,
631 v3_muladds( mtx
[3], mtx
[1], -c
->height
*0.5f
+c
->radius
, p0
);
632 v3_muladds( mtx
[3], mtx
[1], c
->height
*0.5f
-c
->radius
, p1
);
635 if( manifold
->t0
<= 1.0f
){
639 v3_muls( p0
, 1.0f
-manifold
->t0
, pa
);
640 v3_muladds( pa
, p1
, manifold
->t0
, pa
);
642 float d
= v3_length( manifold
->d0
);
643 v3_muls( manifold
->d0
, 1.0f
/d
, ct
->n
);
644 v3_muladds( pa
, ct
->n
, -c
->radius
, ct
->co
);
646 ct
->p
= manifold
->r0
- d
;
647 ct
->type
= k_contact_type_default
;
651 if( (manifold
->t1
>= 0.0f
) && (manifold
->t0
!= manifold
->t1
) ){
652 rb_ct
*ct
= buf
+count
;
655 v3_muls( p0
, 1.0f
-manifold
->t1
, pa
);
656 v3_muladds( pa
, p1
, manifold
->t1
, pa
);
658 float d
= v3_length( manifold
->d1
);
659 v3_muls( manifold
->d1
, 1.0f
/d
, ct
->n
);
660 v3_muladds( pa
, ct
->n
, -c
->radius
, ct
->co
);
662 ct
->p
= manifold
->r1
- d
;
663 ct
->type
= k_contact_type_default
;
673 vg_line( buf
[0].co
, buf
[1].co
, 0xff0000ff );
678 static int rb_capsule_sphere( rb_object
*obja
, rb_object
*objb
, rb_ct
*buf
){
679 rigidbody
*rba
= &obja
->rb
, *rbb
= &objb
->rb
;
680 float h
= obja
->inf
.capsule
.height
,
681 ra
= obja
->inf
.capsule
.radius
,
682 rb
= objb
->inf
.sphere
.radius
;
685 v3_muladds( rba
->co
, rba
->to_world
[1], -h
*0.5f
+ra
, p0
);
686 v3_muladds( rba
->co
, rba
->to_world
[1], h
*0.5f
-ra
, p1
);
689 closest_point_segment( p0
, p1
, rbb
->co
, c
);
690 v3_sub( c
, rbb
->co
, delta
);
692 float d2
= v3_length2(delta
),
699 v3_muls( delta
, 1.0f
/d
, ct
->n
);
703 v3_muladds( c
, ct
->n
, -ra
, p0
);
704 v3_muladds( rbb
->co
, ct
->n
, rb
, p1
);
705 v3_add( p0
, p1
, ct
->co
);
706 v3_muls( ct
->co
, 0.5f
, ct
->co
);
710 ct
->type
= k_contact_type_default
;
718 static int rb_capsule__capsule( m4x3f mtxA
, rb_capsule
*ca
,
719 m4x3f mtxB
, rb_capsule
*cb
, rb_ct
*buf
){
720 float ha
= ca
->height
,
727 v3_muladds( mtxA
[3], mtxA
[1], -ha
*0.5f
+ra
, p0
);
728 v3_muladds( mtxA
[3], mtxA
[1], ha
*0.5f
-ra
, p1
);
729 v3_muladds( mtxB
[3], mtxB
[1], -hb
*0.5f
+rb
, p2
);
730 v3_muladds( mtxB
[3], mtxB
[1], hb
*0.5f
-rb
, p3
);
732 capsule_manifold manifold
;
733 rb_capsule_manifold_init( &manifold
);
737 closest_segment_segment( p0
, p1
, p2
, p3
, &ta
, &tb
, pa
, pb
);
738 rb_capsule_manifold( pa
, pb
, ta
, r
, &manifold
);
740 ta
= closest_point_segment( p0
, p1
, p2
, pa
);
741 tb
= closest_point_segment( p0
, p1
, p3
, pb
);
742 rb_capsule_manifold( pa
, p2
, ta
, r
, &manifold
);
743 rb_capsule_manifold( pb
, p3
, tb
, r
, &manifold
);
745 closest_point_segment( p2
, p3
, p0
, pa
);
746 closest_point_segment( p2
, p3
, p1
, pb
);
747 rb_capsule_manifold( p0
, pa
, 0.0f
, r
, &manifold
);
748 rb_capsule_manifold( p1
, pb
, 1.0f
, r
, &manifold
);
750 return rb_capsule__manifold_done( mtxA
, ca
, &manifold
, buf
);
753 static int rb_sphere_box( rb_object
*obja
, rb_object
*objb
, rb_ct
*buf
){
755 rigidbody
*rba
= &obja
->rb
, *rbb
= &objb
->rb
;
757 closest_point_obb( rba
->co
, rbb
->bbx
, rbb
->to_world
, rbb
->to_local
, co
);
758 v3_sub( rba
->co
, co
, delta
);
760 float d2
= v3_length2(delta
),
761 r
= obja
->inf
.sphere
.radius
;
768 v3_sub( rba
->co
, rbb
->co
, delta
);
771 * some extra testing is required to find the best axis to push the
772 * object back outside the box. Since there isnt a clear seperating
773 * vector already, especially on really high aspect boxes.
775 float lx
= v3_dot( rbb
->to_world
[0], delta
),
776 ly
= v3_dot( rbb
->to_world
[1], delta
),
777 lz
= v3_dot( rbb
->to_world
[2], delta
),
778 px
= rbb
->bbx
[1][0] - fabsf(lx
),
779 py
= rbb
->bbx
[1][1] - fabsf(ly
),
780 pz
= rbb
->bbx
[1][2] - fabsf(lz
);
782 if( px
< py
&& px
< pz
)
783 v3_muls( rbb
->to_world
[0], vg_signf(lx
), ct
->n
);
785 v3_muls( rbb
->to_world
[1], vg_signf(ly
), ct
->n
);
787 v3_muls( rbb
->to_world
[2], vg_signf(lz
), ct
->n
);
789 v3_muladds( rba
->co
, ct
->n
, -r
, ct
->co
);
794 v3_muls( delta
, 1.0f
/d
, ct
->n
);
796 v3_copy( co
, ct
->co
);
801 ct
->type
= k_contact_type_default
;
808 static int rb_sphere_sphere( rb_object
*obja
, rb_object
*objb
, rb_ct
*buf
){
809 rigidbody
*rba
= &obja
->rb
, *rbb
= &objb
->rb
;
811 v3_sub( rba
->co
, rbb
->co
, delta
);
813 float d2
= v3_length2(delta
),
814 r
= obja
->inf
.sphere
.radius
+ objb
->inf
.sphere
.radius
;
820 v3_muls( delta
, 1.0f
/d
, ct
->n
);
823 v3_muladds( rba
->co
, ct
->n
,-obja
->inf
.sphere
.radius
, p0
);
824 v3_muladds( rbb
->co
, ct
->n
, objb
->inf
.sphere
.radius
, p1
);
825 v3_add( p0
, p1
, ct
->co
);
826 v3_muls( ct
->co
, 0.5f
, ct
->co
);
827 ct
->type
= k_contact_type_default
;
837 static int rb_sphere__triangle( m4x3f mtxA
, rb_sphere
*b
,
838 v3f tri
[3], rb_ct
*buf
){
840 enum contact_type type
= closest_on_triangle_1( mtxA
[3], tri
, co
);
842 v3_sub( mtxA
[3], co
, delta
);
844 float d2
= v3_length2( delta
),
851 v3_sub( tri
[2], tri
[0], ab
);
852 v3_sub( tri
[1], tri
[0], ac
);
853 v3_cross( ac
, ab
, tn
);
854 v3_copy( tn
, ct
->n
);
856 if( v3_length2( ct
->n
) <= 0.00001f
){
857 #ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING
858 vg_error( "Zero area triangle!\n" );
863 v3_normalize( ct
->n
);
867 v3_copy( co
, ct
->co
);
876 static int rb_sphere__scene( m4x3f mtxA
, rb_sphere
*b
,
877 m4x3f mtxB
, rb_scene
*s
, rb_ct
*buf
,
879 scene_context
*sc
= s
->bh_scene
->user
;
883 float r
= b
->radius
+ 0.1f
;
885 v3_sub( mtxA
[3], (v3f
){ r
,r
,r
}, box
[0] );
886 v3_add( mtxA
[3], (v3f
){ r
,r
,r
}, box
[1] );
890 bh_iter_init_box( 0, &it
, box
);
892 while( bh_next( s
->bh_scene
, &it
, &idx
) ){
893 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
896 if( sc
->arrvertices
[ptri
[0]].flags
& ignore
) continue;
898 for( int j
=0; j
<3; j
++ )
899 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
901 buf
[ count
].element_id
= ptri
[0];
903 vg_line( tri
[0],tri
[1],0x70ff6000 );
904 vg_line( tri
[1],tri
[2],0x70ff6000 );
905 vg_line( tri
[2],tri
[0],0x70ff6000 );
907 int contact
= rb_sphere__triangle( mtxA
, b
, tri
, &buf
[count
] );
911 vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
919 static int rb_box__scene( m4x3f mtxA
, boxf bbx
,
920 m4x3f mtxB
, rb_scene
*s
, rb_ct
*buf
, u16 ignore
){
921 scene_context
*sc
= s
->bh_scene
->user
;
925 v3_sub( bbx
[1], bbx
[0], extent
);
926 v3_muls( extent
, 0.5f
, extent
);
927 v3_add( bbx
[0], extent
, center
);
929 float r
= v3_length(extent
);
931 v3_fill( world_bbx
[0], -r
);
932 v3_fill( world_bbx
[1], r
);
933 for( int i
=0; i
<2; i
++ ){
934 v3_add( center
, world_bbx
[i
], world_bbx
[i
] );
935 v3_add( mtxA
[3], world_bbx
[i
], world_bbx
[i
] );
939 m4x3_invert_affine( mtxA
, to_local
);
942 bh_iter_init_box( 0, &it
, world_bbx
);
946 vg_line_boxf( world_bbx
, VG__RED
);
948 while( bh_next( s
->bh_scene
, &it
, &idx
) ){
949 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
950 if( sc
->arrvertices
[ptri
[0]].flags
& ignore
) continue;
952 for( int j
=0; j
<3; j
++ )
953 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
955 if( rb_box_triangle_sat( extent
, center
, to_local
, tri
) ){
956 vg_line(tri
[0],tri
[1],0xff50ff00 );
957 vg_line(tri
[1],tri
[2],0xff50ff00 );
958 vg_line(tri
[2],tri
[0],0xff50ff00 );
961 vg_line(tri
[0],tri
[1],0xff0000ff );
962 vg_line(tri
[1],tri
[2],0xff0000ff );
963 vg_line(tri
[2],tri
[0],0xff0000ff );
968 v3_sub( tri
[1], tri
[0], v0
);
969 v3_sub( tri
[2], tri
[0], v1
);
970 v3_cross( v0
, v1
, n
);
972 if( v3_length2( n
) <= 0.00001f
){
973 #ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING
974 vg_error( "Zero area triangle!\n" );
981 /* find best feature */
982 float best
= v3_dot( mtxA
[0], n
);
985 for( int i
=1; i
<3; i
++ ){
986 float c
= v3_dot( mtxA
[i
], n
);
988 if( fabsf(c
) > fabsf(best
) ){
997 float px
= best
> 0.0f
? bbx
[0][0]: bbx
[1][0];
999 manifold
[0][1] = bbx
[0][1];
1000 manifold
[0][2] = bbx
[0][2];
1001 manifold
[1][0] = px
;
1002 manifold
[1][1] = bbx
[1][1];
1003 manifold
[1][2] = bbx
[0][2];
1004 manifold
[2][0] = px
;
1005 manifold
[2][1] = bbx
[1][1];
1006 manifold
[2][2] = bbx
[1][2];
1007 manifold
[3][0] = px
;
1008 manifold
[3][1] = bbx
[0][1];
1009 manifold
[3][2] = bbx
[1][2];
1011 else if( axis
== 1 ){
1012 float py
= best
> 0.0f
? bbx
[0][1]: bbx
[1][1];
1013 manifold
[0][0] = bbx
[0][0];
1014 manifold
[0][1] = py
;
1015 manifold
[0][2] = bbx
[0][2];
1016 manifold
[1][0] = bbx
[1][0];
1017 manifold
[1][1] = py
;
1018 manifold
[1][2] = bbx
[0][2];
1019 manifold
[2][0] = bbx
[1][0];
1020 manifold
[2][1] = py
;
1021 manifold
[2][2] = bbx
[1][2];
1022 manifold
[3][0] = bbx
[0][0];
1023 manifold
[3][1] = py
;
1024 manifold
[3][2] = bbx
[1][2];
1027 float pz
= best
> 0.0f
? bbx
[0][2]: bbx
[1][2];
1028 manifold
[0][0] = bbx
[0][0];
1029 manifold
[0][1] = bbx
[0][1];
1030 manifold
[0][2] = pz
;
1031 manifold
[1][0] = bbx
[1][0];
1032 manifold
[1][1] = bbx
[0][1];
1033 manifold
[1][2] = pz
;
1034 manifold
[2][0] = bbx
[1][0];
1035 manifold
[2][1] = bbx
[1][1];
1036 manifold
[2][2] = pz
;
1037 manifold
[3][0] = bbx
[0][0];
1038 manifold
[3][1] = bbx
[1][1];
1039 manifold
[3][2] = pz
;
1042 for( int j
=0; j
<4; j
++ )
1043 m4x3_mulv( mtxA
, manifold
[j
], manifold
[j
] );
1045 vg_line( manifold
[0], manifold
[1], 0xffffffff );
1046 vg_line( manifold
[1], manifold
[2], 0xffffffff );
1047 vg_line( manifold
[2], manifold
[3], 0xffffffff );
1048 vg_line( manifold
[3], manifold
[0], 0xffffffff );
1050 for( int j
=0; j
<4; j
++ ){
1051 rb_ct
*ct
= buf
+count
;
1053 v3_copy( manifold
[j
], ct
->co
);
1054 v3_copy( n
, ct
->n
);
1056 float l0
= v3_dot( tri
[0], n
),
1057 l1
= v3_dot( manifold
[j
], n
);
1059 ct
->p
= (l0
-l1
)*0.5f
;
1063 ct
->type
= k_contact_type_default
;
1073 static int rb_capsule__triangle( m4x3f mtxA
, rb_capsule
*c
,
1074 v3f tri
[3], rb_ct
*buf
){
1076 v3_muladds( mtxA
[3], mtxA
[1], -c
->height
*0.5f
+c
->radius
, p0w
);
1077 v3_muladds( mtxA
[3], mtxA
[1], c
->height
*0.5f
-c
->radius
, p1w
);
1079 capsule_manifold manifold
;
1080 rb_capsule_manifold_init( &manifold
);
1083 v3_sub( tri
[1], tri
[0], v0
);
1084 v3_sub( tri
[2], tri
[0], v1
);
1085 v3_cross( v0
, v1
, n
);
1087 if( v3_length2( n
) <= 0.00001f
){
1088 #ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING
1089 vg_error( "Zero area triangle!\n" );
1097 /* deep penetration recovery. for when we clip through the triangles. so its
1098 * not very 'correct' */
1100 if( ray_tri( tri
, p0w
, mtxA
[1], &dist
, 1 ) ){
1101 f32 l
= c
->height
- c
->radius
*2.0f
;
1102 if( (dist
>= 0.0f
) && (dist
< l
) ){
1104 v3_muladds( p0w
, mtxA
[1], dist
, co
);
1105 vg_line_point( co
, 0.02f
, 0xffffff00 );
1108 v3_sub( p0w
, co
, d0
);
1109 v3_sub( p1w
, co
, d1
);
1111 f32 p
= vg_minf( v3_dot( n
, d0
), v3_dot( n
, d1
) ) - c
->radius
;
1115 ct
->type
= k_contact_type_default
;
1116 v3_copy( n
, ct
->n
);
1117 v3_muladds( co
, n
, p
, ct
->co
);
1125 closest_on_triangle_1( p0w
, tri
, c0
);
1126 closest_on_triangle_1( p1w
, tri
, c1
);
1129 v3_sub( c0
, p0w
, d0
);
1130 v3_sub( c1
, p1w
, d1
);
1131 v3_sub( p1w
, p0w
, da
);
1137 /* the two balls at the ends */
1138 if( v3_dot( da
, d0
) <= 0.01f
)
1139 rb_capsule_manifold( p0w
, c0
, 0.0f
, c
->radius
, &manifold
);
1140 if( v3_dot( da
, d1
) >= -0.01f
)
1141 rb_capsule_manifold( p1w
, c1
, 1.0f
, c
->radius
, &manifold
);
1143 /* the edges to edges */
1144 for( int i
=0; i
<3; i
++ ){
1150 closest_segment_segment( p0w
, p1w
, tri
[i0
], tri
[i1
], &ta
, &tb
, ca
, cb
);
1151 rb_capsule_manifold( ca
, cb
, ta
, c
->radius
, &manifold
);
1154 int count
= rb_capsule__manifold_done( mtxA
, c
, &manifold
, buf
);
1155 for( int i
=0; i
<count
; i
++ )
1156 v3_copy( n
, buf
[i
].n
);
1161 /* mtxB is defined only for tradition; it is not used currently */
1162 static int rb_capsule__scene( m4x3f mtxA
, rb_capsule
*c
,
1163 m4x3f mtxB
, rb_scene
*s
,
1164 rb_ct
*buf
, u16 ignore
){
1168 v3_sub( mtxA
[3], (v3f
){ c
->height
, c
->height
, c
->height
}, bbx
[0] );
1169 v3_add( mtxA
[3], (v3f
){ c
->height
, c
->height
, c
->height
}, bbx
[1] );
1171 scene_context
*sc
= s
->bh_scene
->user
;
1174 bh_iter_init_box( 0, &it
, bbx
);
1176 while( bh_next( s
->bh_scene
, &it
, &idx
) ){
1177 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
1178 if( sc
->arrvertices
[ptri
[0]].flags
& ignore
) continue;
1181 for( int j
=0; j
<3; j
++ )
1182 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
1184 buf
[ count
].element_id
= ptri
[0];
1186 int contact
= rb_capsule__triangle( mtxA
, c
, tri
, &buf
[count
] );
1190 vg_warn("Exceeding capsule_vs_scene capacity. Geometry too dense!\n");
1198 static int rb_global_has_space( void ){
1199 if( rb_contact_count
+ 16 > vg_list_size(rb_contact_buffer
) )
1205 static rb_ct
*rb_global_buffer( void ){
1206 return &rb_contact_buffer
[ rb_contact_count
];
1210 * -----------------------------------------------------------------------------
1212 * -----------------------------------------------------------------------------
1215 static void rb_solver_reset(void){
1216 rb_contact_count
= 0;
1219 static rb_ct
*rb_global_ct(void){
1220 return rb_contact_buffer
+ rb_contact_count
;
1223 static void rb_prepare_contact( rb_ct
*ct
, float timestep
){
1224 ct
->bias
= -0.2f
* (timestep
*3600.0f
)
1225 * vg_minf( 0.0f
, -ct
->p
+k_penetration_slop
);
1227 v3_tangent_basis( ct
->n
, ct
->t
[0], ct
->t
[1] );
1228 ct
->norm_impulse
= 0.0f
;
1229 ct
->tangent_impulse
[0] = 0.0f
;
1230 ct
->tangent_impulse
[1] = 0.0f
;
1233 /* calculate total move. manifold should belong to ONE object only */
1234 static void rb_depenetrate( rb_ct
*manifold
, int len
, v3f dt
){
1237 for( int j
=0; j
<7; j
++ )
1239 for( int i
=0; i
<len
; i
++ )
1241 struct contact
*ct
= &manifold
[i
];
1243 float resolved_amt
= v3_dot( ct
->n
, dt
),
1244 remaining
= (ct
->p
-k_penetration_slop
) - resolved_amt
,
1245 apply
= vg_maxf( remaining
, 0.0f
) * 0.4f
;
1247 v3_muladds( dt
, ct
->n
, apply
, dt
);
1253 * Initializing things like tangent vectors
1255 static void rb_presolve_contacts( rb_ct
*buffer
, int len
){
1256 for( int i
=0; i
<len
; i
++ ){
1257 rb_ct
*ct
= &buffer
[i
];
1258 rb_prepare_contact( ct
, k_rb_delta
);
1260 v3f ra
, rb
, raCn
, rbCn
, raCt
, rbCt
;
1261 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
1262 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
1263 v3_cross( ra
, ct
->n
, raCn
);
1264 v3_cross( rb
, ct
->n
, rbCn
);
1266 /* orient inverse inertia tensors */
1268 m3x3_mulv( ct
->rba
->iIw
, raCn
, raCnI
);
1269 m3x3_mulv( ct
->rbb
->iIw
, rbCn
, rbCnI
);
1271 ct
->normal_mass
= ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
1272 ct
->normal_mass
+= v3_dot( raCn
, raCnI
);
1273 ct
->normal_mass
+= v3_dot( rbCn
, rbCnI
);
1274 ct
->normal_mass
= 1.0f
/ct
->normal_mass
;
1276 for( int j
=0; j
<2; j
++ ){
1278 v3_cross( ct
->t
[j
], ra
, raCt
);
1279 v3_cross( ct
->t
[j
], rb
, rbCt
);
1280 m3x3_mulv( ct
->rba
->iIw
, raCt
, raCtI
);
1281 m3x3_mulv( ct
->rbb
->iIw
, rbCt
, rbCtI
);
1283 ct
->tangent_mass
[j
] = ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
1284 ct
->tangent_mass
[j
] += v3_dot( raCt
, raCtI
);
1285 ct
->tangent_mass
[j
] += v3_dot( rbCt
, rbCtI
);
1286 ct
->tangent_mass
[j
] = 1.0f
/ct
->tangent_mass
[j
];
1289 rb_debug_contact( ct
);
1294 * Creates relative contact velocity vector
1296 static void rb_rcv( rigidbody
*rba
, rigidbody
*rbb
, v3f ra
, v3f rb
, v3f rv
){
1298 v3_cross( rba
->w
, ra
, rva
);
1299 v3_add( rba
->v
, rva
, rva
);
1300 v3_cross( rbb
->w
, rb
, rvb
);
1301 v3_add( rbb
->v
, rvb
, rvb
);
1303 v3_sub( rva
, rvb
, rv
);
1306 static void rb_contact_restitution( rb_ct
*ct
, float cr
){
1308 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
1309 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
1310 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
1312 float v
= v3_dot( rv
, ct
->n
);
1315 ct
->bias
+= -cr
* v
;
1320 * Apply impulse to object
1322 static void rb_linear_impulse( rigidbody
*rb
, v3f delta
, v3f impulse
){
1324 v3_muladds( rb
->v
, impulse
, rb
->inv_mass
, rb
->v
);
1326 /* Angular velocity */
1328 v3_cross( delta
, impulse
, wa
);
1330 m3x3_mulv( rb
->iIw
, wa
, wa
);
1331 v3_add( rb
->w
, wa
, rb
->w
);
1335 * One iteration to solve the contact constraint
1337 static void rb_solve_contacts( rb_ct
*buf
, int len
){
1338 for( int i
=0; i
<len
; i
++ ){
1339 struct contact
*ct
= &buf
[i
];
1342 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
1343 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
1344 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
1347 for( int j
=0; j
<2; j
++ ){
1348 float f
= k_friction
* ct
->norm_impulse
,
1349 vt
= v3_dot( rv
, ct
->t
[j
] ),
1350 lambda
= ct
->tangent_mass
[j
] * -vt
;
1352 float temp
= ct
->tangent_impulse
[j
];
1353 ct
->tangent_impulse
[j
] = vg_clampf( temp
+ lambda
, -f
, f
);
1354 lambda
= ct
->tangent_impulse
[j
] - temp
;
1357 v3_muls( ct
->t
[j
], lambda
, impulse
);
1358 rb_linear_impulse( ct
->rba
, ra
, impulse
);
1360 v3_muls( ct
->t
[j
], -lambda
, impulse
);
1361 rb_linear_impulse( ct
->rbb
, rb
, impulse
);
1365 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
1366 float vn
= v3_dot( rv
, ct
->n
),
1367 lambda
= ct
->normal_mass
* (-vn
+ ct
->bias
);
1369 float temp
= ct
->norm_impulse
;
1370 ct
->norm_impulse
= vg_maxf( temp
+ lambda
, 0.0f
);
1371 lambda
= ct
->norm_impulse
- temp
;
1374 v3_muls( ct
->n
, lambda
, impulse
);
1375 rb_linear_impulse( ct
->rba
, ra
, impulse
);
1377 v3_muls( ct
->n
, -lambda
, impulse
);
1378 rb_linear_impulse( ct
->rbb
, rb
, impulse
);
1383 * -----------------------------------------------------------------------------
1385 * -----------------------------------------------------------------------------
1388 static void rb_debug_position_constraints( rb_constr_pos
*buffer
, int len
){
1389 for( int i
=0; i
<len
; i
++ ){
1390 rb_constr_pos
*constr
= &buffer
[i
];
1391 rigidbody
*rba
= constr
->rba
, *rbb
= constr
->rbb
;
1394 m3x3_mulv( rba
->to_world
, constr
->lca
, wca
);
1395 m3x3_mulv( rbb
->to_world
, constr
->lcb
, wcb
);
1398 v3_add( wca
, rba
->co
, p0
);
1399 v3_add( wcb
, rbb
->co
, p1
);
1400 vg_line_point( p0
, 0.0025f
, 0xff000000 );
1401 vg_line_point( p1
, 0.0025f
, 0xffffffff );
1402 vg_line2( p0
, p1
, 0xff000000, 0xffffffff );
1406 static void rb_presolve_swingtwist_constraints( rb_constr_swingtwist
*buf
,
1410 for( int i
=0; i
<len
; i
++ ){
1411 rb_constr_swingtwist
*st
= &buf
[ i
];
1413 v3f vx
, vy
, va
, vxb
, axis
, center
;
1415 m3x3_mulv( st
->rba
->to_world
, st
->conevx
, vx
);
1416 m3x3_mulv( st
->rbb
->to_world
, st
->conevxb
, vxb
);
1417 m3x3_mulv( st
->rba
->to_world
, st
->conevy
, vy
);
1418 m3x3_mulv( st
->rbb
->to_world
, st
->coneva
, va
);
1419 m4x3_mulv( st
->rba
->to_world
, st
->view_offset
, center
);
1420 v3_cross( vy
, vx
, axis
);
1422 /* Constraint violated ? */
1423 float fx
= v3_dot( vx
, va
), /* projection world */
1424 fy
= v3_dot( vy
, va
),
1425 fn
= v3_dot( va
, axis
),
1427 rx
= st
->conevx
[3], /* elipse radii */
1430 lx
= fx
/rx
, /* projection local (fn==lz) */
1433 st
->tangent_violation
= ((lx
*lx
+ ly
*ly
) > fn
*fn
) || (fn
<= 0.0f
);
1434 if( st
->tangent_violation
){
1435 /* Calculate a good position and the axis to solve on */
1436 v2f closest
, tangent
,
1437 p
= { fx
/fabsf(fn
), fy
/fabsf(fn
) };
1439 closest_point_elipse( p
, (v2f
){rx
,ry
}, closest
);
1440 tangent
[0] = -closest
[1] / (ry
*ry
);
1441 tangent
[1] = closest
[0] / (rx
*rx
);
1442 v2_normalize( tangent
);
1445 v3_muladds( axis
, vx
, closest
[0], v0
);
1446 v3_muladds( v0
, vy
, closest
[1], v0
);
1449 v3_muls( vx
, tangent
[0], v1
);
1450 v3_muladds( v1
, vy
, tangent
[1], v1
);
1452 v3_copy( v0
, st
->tangent_target
);
1453 v3_copy( v1
, st
->tangent_axis
);
1455 /* calculate mass */
1457 m3x3_mulv( st
->rba
->iIw
, st
->tangent_axis
, aIw
);
1458 m3x3_mulv( st
->rbb
->iIw
, st
->tangent_axis
, bIw
);
1459 st
->tangent_mass
= 1.0f
/ (v3_dot( st
->tangent_axis
, aIw
) +
1460 v3_dot( st
->tangent_axis
, bIw
));
1462 float angle
= v3_dot( va
, st
->tangent_target
);
1466 v3_cross( vy
, va
, refaxis
); /* our default rotation */
1467 v3_normalize( refaxis
);
1469 float angle
= v3_dot( refaxis
, vxb
);
1470 st
->axis_violation
= fabsf(angle
) < st
->conet
;
1472 if( st
->axis_violation
){
1474 v3_cross( refaxis
, vxb
, dir_test
);
1476 if( v3_dot(dir_test
, va
) < 0.0f
)
1477 st
->axis_violation
= -st
->axis_violation
;
1479 float newang
= (float)st
->axis_violation
* acosf(st
->conet
-0.0001f
);
1482 v3_cross( va
, refaxis
, refaxis_up
);
1483 v3_muls( refaxis_up
, sinf(newang
), st
->axis_target
);
1484 v3_muladds( st
->axis_target
, refaxis
, -cosf(newang
), st
->axis_target
);
1486 /* calculate mass */
1487 v3_copy( va
, st
->axis
);
1489 m3x3_mulv( st
->rba
->iIw
, st
->axis
, aIw
);
1490 m3x3_mulv( st
->rbb
->iIw
, st
->axis
, bIw
);
1491 st
->axis_mass
= 1.0f
/ (v3_dot( st
->axis
, aIw
) +
1492 v3_dot( st
->axis
, bIw
));
1497 static void rb_debug_swingtwist_constraints( rb_constr_swingtwist
*buf
,
1501 for( int i
=0; i
<len
; i
++ ){
1502 rb_constr_swingtwist
*st
= &buf
[ i
];
1504 v3f vx
, vxb
, vy
, va
, axis
, center
;
1506 m3x3_mulv( st
->rba
->to_world
, st
->conevx
, vx
);
1507 m3x3_mulv( st
->rbb
->to_world
, st
->conevxb
, vxb
);
1508 m3x3_mulv( st
->rba
->to_world
, st
->conevy
, vy
);
1509 m3x3_mulv( st
->rbb
->to_world
, st
->coneva
, va
);
1510 m4x3_mulv( st
->rba
->to_world
, st
->view_offset
, center
);
1511 v3_cross( vy
, vx
, axis
);
1513 float rx
= st
->conevx
[3], /* elipse radii */
1517 v3_muladds( center
, va
, size
, p1
);
1518 vg_line( center
, p1
, 0xffffffff );
1519 vg_line_point( p1
, 0.00025f
, 0xffffffff );
1521 if( st
->tangent_violation
){
1522 v3_muladds( center
, st
->tangent_target
, size
, p0
);
1524 vg_line( center
, p0
, 0xff00ff00 );
1525 vg_line_point( p0
, 0.00025f
, 0xff00ff00 );
1526 vg_line( p1
, p0
, 0xff000000 );
1529 for( int x
=0; x
<32; x
++ ){
1530 float t0
= ((float)x
* (1.0f
/32.0f
)) * VG_TAUf
,
1531 t1
= (((float)x
+1.0f
) * (1.0f
/32.0f
)) * VG_TAUf
,
1538 v3_muladds( axis
, vx
, c0
*rx
, v0
);
1539 v3_muladds( v0
, vy
, s0
*ry
, v0
);
1540 v3_muladds( axis
, vx
, c1
*rx
, v1
);
1541 v3_muladds( v1
, vy
, s1
*ry
, v1
);
1546 v3_muladds( center
, v0
, size
, p0
);
1547 v3_muladds( center
, v1
, size
, p1
);
1549 u32 col0r
= fabsf(c0
) * 255.0f
,
1550 col0g
= fabsf(s0
) * 255.0f
,
1551 col1r
= fabsf(c1
) * 255.0f
,
1552 col1g
= fabsf(s1
) * 255.0f
,
1553 col
= st
->tangent_violation
? 0xff0000ff: 0xff000000,
1554 col0
= col
| (col0r
<<16) | (col0g
<< 8),
1555 col1
= col
| (col1r
<<16) | (col1g
<< 8);
1557 vg_line2( center
, p0
, VG__NONE
, col0
);
1558 vg_line2( p0
, p1
, col0
, col1
);
1562 v3_muladds( center
, va
, size
, p0
);
1563 v3_muladds( p0
, vxb
, size
, p1
);
1565 vg_line( p0
, p1
, 0xff0000ff );
1567 if( st
->axis_violation
){
1568 v3_muladds( p0
, st
->axis_target
, size
*1.25f
, p1
);
1569 vg_line( p0
, p1
, 0xffffff00 );
1570 vg_line_point( p1
, 0.0025f
, 0xffffff80 );
1574 v3_cross( vy
, va
, refaxis
); /* our default rotation */
1575 v3_normalize( refaxis
);
1577 v3_cross( va
, refaxis
, refaxis_up
);
1578 float newang
= acosf(st
->conet
-0.0001f
);
1580 v3_muladds( p0
, refaxis_up
, sinf(newang
)*size
, p1
);
1581 v3_muladds( p1
, refaxis
, -cosf(newang
)*size
, p1
);
1582 vg_line( p0
, p1
, 0xff000000 );
1584 v3_muladds( p0
, refaxis_up
, sinf(-newang
)*size
, p1
);
1585 v3_muladds( p1
, refaxis
, -cosf(-newang
)*size
, p1
);
1586 vg_line( p0
, p1
, 0xff404040 );
1591 * Solve a list of positional constraints
1593 static void rb_solve_position_constraints( rb_constr_pos
*buf
, int len
){
1594 for( int i
=0; i
<len
; i
++ ){
1595 rb_constr_pos
*constr
= &buf
[i
];
1596 rigidbody
*rba
= constr
->rba
, *rbb
= constr
->rbb
;
1599 m3x3_mulv( rba
->to_world
, constr
->lca
, wa
);
1600 m3x3_mulv( rbb
->to_world
, constr
->lcb
, wb
);
1602 m3x3f ssra
, ssrat
, ssrb
, ssrbt
;
1604 m3x3_skew_symetric( ssrat
, wa
);
1605 m3x3_skew_symetric( ssrbt
, wb
);
1606 m3x3_transpose( ssrat
, ssra
);
1607 m3x3_transpose( ssrbt
, ssrb
);
1609 v3f b
, b_wa
, b_wb
, b_a
, b_b
;
1610 m3x3_mulv( ssra
, rba
->w
, b_wa
);
1611 m3x3_mulv( ssrb
, rbb
->w
, b_wb
);
1612 v3_add( rba
->v
, b_wa
, b
);
1613 v3_sub( b
, rbb
->v
, b
);
1614 v3_sub( b
, b_wb
, b
);
1615 v3_muls( b
, -1.0f
, b
);
1618 m3x3_diagonal( invMa
, rba
->inv_mass
);
1619 m3x3_diagonal( invMb
, rbb
->inv_mass
);
1622 m3x3_mul( ssra
, rba
->iIw
, ia
);
1623 m3x3_mul( ia
, ssrat
, ia
);
1624 m3x3_mul( ssrb
, rbb
->iIw
, ib
);
1625 m3x3_mul( ib
, ssrbt
, ib
);
1628 m3x3_add( invMa
, ia
, cma
);
1629 m3x3_add( invMb
, ib
, cmb
);
1632 m3x3_add( cma
, cmb
, A
);
1634 /* Solve Ax = b ( A^-1*b = x ) */
1637 m3x3_inv( A
, invA
);
1638 m3x3_mulv( invA
, b
, impulse
);
1640 v3f delta_va
, delta_wa
, delta_vb
, delta_wb
;
1642 m3x3_mul( rba
->iIw
, ssrat
, iwa
);
1643 m3x3_mul( rbb
->iIw
, ssrbt
, iwb
);
1645 m3x3_mulv( invMa
, impulse
, delta_va
);
1646 m3x3_mulv( invMb
, impulse
, delta_vb
);
1647 m3x3_mulv( iwa
, impulse
, delta_wa
);
1648 m3x3_mulv( iwb
, impulse
, delta_wb
);
1650 v3_add( rba
->v
, delta_va
, rba
->v
);
1651 v3_add( rba
->w
, delta_wa
, rba
->w
);
1652 v3_sub( rbb
->v
, delta_vb
, rbb
->v
);
1653 v3_sub( rbb
->w
, delta_wb
, rbb
->w
);
1657 static void rb_solve_swingtwist_constraints( rb_constr_swingtwist
*buf
,
1661 for( int i
=0; i
<len
; i
++ ){
1662 rb_constr_swingtwist
*st
= &buf
[ i
];
1664 if( !st
->axis_violation
)
1667 float rv
= v3_dot( st
->axis
, st
->rbb
->w
) -
1668 v3_dot( st
->axis
, st
->rba
->w
);
1670 if( rv
* (float)st
->axis_violation
> 0.0f
)
1673 v3f impulse
, wa
, wb
;
1674 v3_muls( st
->axis
, rv
*st
->axis_mass
, impulse
);
1675 m3x3_mulv( st
->rba
->iIw
, impulse
, wa
);
1676 v3_add( st
->rba
->w
, wa
, st
->rba
->w
);
1678 v3_muls( impulse
, -1.0f
, impulse
);
1679 m3x3_mulv( st
->rbb
->iIw
, impulse
, wb
);
1680 v3_add( st
->rbb
->w
, wb
, st
->rbb
->w
);
1682 float rv2
= v3_dot( st
->axis
, st
->rbb
->w
) -
1683 v3_dot( st
->axis
, st
->rba
->w
);
1686 for( int i
=0; i
<len
; i
++ ){
1687 rb_constr_swingtwist
*st
= &buf
[ i
];
1689 if( !st
->tangent_violation
)
1692 float rv
= v3_dot( st
->tangent_axis
, st
->rbb
->w
) -
1693 v3_dot( st
->tangent_axis
, st
->rba
->w
);
1698 v3f impulse
, wa
, wb
;
1699 v3_muls( st
->tangent_axis
, rv
*st
->tangent_mass
, impulse
);
1700 m3x3_mulv( st
->rba
->iIw
, impulse
, wa
);
1701 v3_add( st
->rba
->w
, wa
, st
->rba
->w
);
1703 v3_muls( impulse
, -1.0f
, impulse
);
1704 m3x3_mulv( st
->rbb
->iIw
, impulse
, wb
);
1705 v3_add( st
->rbb
->w
, wb
, st
->rbb
->w
);
1707 float rv2
= v3_dot( st
->tangent_axis
, st
->rbb
->w
) -
1708 v3_dot( st
->tangent_axis
, st
->rba
->w
);
1712 static void rb_solve_constr_angle( rigidbody
*rba
, rigidbody
*rbb
,
1714 m3x3f ssra
, ssrb
, ssrat
, ssrbt
;
1717 m3x3_skew_symetric( ssrat
, ra
);
1718 m3x3_skew_symetric( ssrbt
, rb
);
1719 m3x3_transpose( ssrat
, ssra
);
1720 m3x3_transpose( ssrbt
, ssrb
);
1722 m3x3_mul( ssra
, rba
->iIw
, cma
);
1723 m3x3_mul( cma
, ssrat
, cma
);
1724 m3x3_mul( ssrb
, rbb
->iIw
, cmb
);
1725 m3x3_mul( cmb
, ssrbt
, cmb
);
1728 m3x3_add( cma
, cmb
, A
);
1729 m3x3_inv( A
, invA
);
1732 m3x3_mulv( ssra
, rba
->w
, b_wa
);
1733 m3x3_mulv( ssrb
, rbb
->w
, b_wb
);
1734 v3_add( b_wa
, b_wb
, b
);
1738 m3x3_mulv( invA
, b
, impulse
);
1740 v3f delta_wa
, delta_wb
;
1742 m3x3_mul( rba
->iIw
, ssrat
, iwa
);
1743 m3x3_mul( rbb
->iIw
, ssrbt
, iwb
);
1744 m3x3_mulv( iwa
, impulse
, delta_wa
);
1745 m3x3_mulv( iwb
, impulse
, delta_wb
);
1746 v3_add( rba
->w
, delta_wa
, rba
->w
);
1747 v3_sub( rbb
->w
, delta_wb
, rbb
->w
);
1751 * Correct position constraint drift errors
1752 * [ 0.0 <= amt <= 1.0 ]: the correction amount
1754 static void rb_correct_position_constraints( rb_constr_pos
*buf
, int len
,
1756 for( int i
=0; i
<len
; i
++ ){
1757 rb_constr_pos
*constr
= &buf
[i
];
1758 rigidbody
*rba
= constr
->rba
, *rbb
= constr
->rbb
;
1761 m3x3_mulv( rba
->to_world
, constr
->lca
, p0
);
1762 m3x3_mulv( rbb
->to_world
, constr
->lcb
, p1
);
1763 v3_add( rba
->co
, p0
, p0
);
1764 v3_add( rbb
->co
, p1
, p1
);
1765 v3_sub( p1
, p0
, d
);
1767 v3_muladds( rbb
->co
, d
, -1.0f
* amt
, rbb
->co
);
1768 rb_update_transform( rbb
);
1772 static void rb_correct_swingtwist_constraints( rb_constr_swingtwist
*buf
,
1773 int len
, float amt
){
1774 for( int i
=0; i
<len
; i
++ ){
1775 rb_constr_swingtwist
*st
= &buf
[i
];
1777 if( !st
->tangent_violation
)
1781 m3x3_mulv( st
->rbb
->to_world
, st
->coneva
, va
);
1783 float angle
= v3_dot( va
, st
->tangent_target
);
1785 if( fabsf(angle
) < 0.9999f
){
1787 v3_cross( va
, st
->tangent_target
, axis
);
1790 q_axis_angle( correction
, axis
, acosf(angle
) * amt
);
1791 q_mul( correction
, st
->rbb
->q
, st
->rbb
->q
);
1792 rb_update_transform( st
->rbb
);
1796 for( int i
=0; i
<len
; i
++ ){
1797 rb_constr_swingtwist
*st
= &buf
[i
];
1799 if( !st
->axis_violation
)
1803 m3x3_mulv( st
->rbb
->to_world
, st
->conevxb
, vxb
);
1805 float angle
= v3_dot( vxb
, st
->axis_target
);
1807 if( fabsf(angle
) < 0.9999f
){
1809 v3_cross( vxb
, st
->axis_target
, axis
);
1812 q_axis_angle( correction
, axis
, acosf(angle
) * amt
);
1813 q_mul( correction
, st
->rbb
->q
, st
->rbb
->q
);
1814 rb_update_transform( st
->rbb
);
1819 static void rb_correct_contact_constraints( rb_ct
*buf
, int len
, float amt
){
1820 for( int i
=0; i
<len
; i
++ ){
1821 rb_ct
*ct
= &buf
[i
];
1822 rigidbody
*rba
= ct
->rba
,
1825 f32 mass_total
= 1.0f
/ (rba
->inv_mass
+ rbb
->inv_mass
),
1826 d
= ct
->p
*mass_total
*amt
;
1828 v3_muladds( rba
->co
, ct
->n
, -d
* rba
->inv_mass
, rba
->co
);
1829 v3_muladds( rbb
->co
, ct
->n
, d
* rbb
->inv_mass
, rbb
->co
);
1838 static void rb_effect_simple_bouyency( rigidbody
*ra
, v4f plane
,
1839 float amt
, float drag
){
1841 float depth
= v3_dot( plane
, ra
->co
) - plane
[3],
1842 lambda
= vg_clampf( -depth
, 0.0f
, 1.0f
) * amt
;
1844 v3_muladds( ra
->v
, plane
, lambda
* k_rb_delta
, ra
->v
);
1847 v3_muls( ra
->v
, 1.0f
-(drag
*k_rb_delta
), ra
->v
);
1850 /* apply a spring&dampener force to match ra(worldspace) on rigidbody, to
1853 static void rb_effect_spring_target_vector( rigidbody
*rba
, v3f ra
, v3f rt
,
1854 float spring
, float dampening
,
1856 float d
= v3_dot( rt
, ra
);
1857 float a
= acosf( vg_clampf( d
, -1.0f
, 1.0f
) );
1860 v3_cross( rt
, ra
, axis
);
1862 float Fs
= -a
* spring
,
1863 Fd
= -v3_dot( rba
->w
, axis
) * dampening
;
1865 v3_muladds( rba
->w
, axis
, (Fs
+Fd
) * timestep
, rba
->w
);
1868 #endif /* RIGIDBODY_H */