2 * Copyright (C) 2021-2023 Mt.ZERO Software, Harry Godden - All Rights Reserved
6 * Resources: Box2D - Erin Catto
10 #include "vg/vg_console.h"
20 * -----------------------------------------------------------------------------
22 * -----------------------------------------------------------------------------
26 k_rb_rate
= (1.0/VG_TIMESTEP_FIXED
),
27 k_rb_delta
= (1.0/k_rb_rate
),
29 k_damp_linear
= 0.1f
, /* scale velocity 1/(1+x) */
30 k_damp_angular
= 0.1f
, /* scale angular 1/(1+x) */
31 k_penetration_slop
= 0.01f
,
32 k_inertia_scale
= 4.0f
,
33 k_phys_baumgarte
= 0.2f
,
39 k_joint_correction
= 0.01f
,
40 k_joint_impulse
= 1.0f
,
41 k_joint_bias
= 0.08f
; /* positional joints */
43 static void rb_register_cvar(void){
44 VG_VAR_F32( k_limit_bias
, flags
=VG_VAR_CHEAT
);
45 VG_VAR_F32( k_joint_bias
, flags
=VG_VAR_CHEAT
);
46 VG_VAR_F32( k_joint_correction
, flags
=VG_VAR_CHEAT
);
47 VG_VAR_F32( k_joint_impulse
, flags
=VG_VAR_CHEAT
);
53 k_rb_shape_sphere
= 2,
54 k_rb_shape_capsule
= 3,
58 * -----------------------------------------------------------------------------
59 * structure definitions
60 * -----------------------------------------------------------------------------
63 typedef struct rigidbody rigidbody
;
64 typedef struct contact rb_ct
;
65 typedef struct rb_capsule rb_capsule
;
77 m3x3f iI
, iIw
; /* inertia model and inverse world tensor */
78 m4x3f to_world
, to_local
;
81 static struct contact
{
85 float p
, bias
, norm_impulse
, tangent_impulse
[2],
86 normal_mass
, tangent_mass
[2];
90 enum contact_type type
;
92 rb_contact_buffer
[256];
93 static int rb_contact_count
= 0;
95 typedef struct rb_constr_pos rb_constr_pos
;
96 typedef struct rb_constr_swingtwist rb_constr_swingtwist
;
103 struct rb_constr_swingtwist
{
104 rigidbody
*rba
, *rbb
;
106 v4f conevx
, conevy
; /* relative to rba */
107 v3f view_offset
, /* relative to rba */
108 coneva
, conevxb
;/* relative to rbb */
110 int tangent_violation
, axis_violation
;
111 v3f axis
, tangent_axis
, tangent_target
, axis_target
;
114 float tangent_mass
, axis_mass
;
116 f32 conv_tangent
, conv_axis
;
120 * -----------------------------------------------------------------------------
122 * -----------------------------------------------------------------------------
125 static void rb_debug_contact( rb_ct
*ct
){
127 v3_muladds( ct
->co
, ct
->n
, 0.05f
, p1
);
129 if( ct
->type
== k_contact_type_default
){
130 vg_line_point( ct
->co
, 0.0125f
, 0xff0000ff );
131 vg_line( ct
->co
, p1
, 0xffffffff );
133 else if( ct
->type
== k_contact_type_edge
){
134 vg_line_point( ct
->co
, 0.0125f
, 0xff00ffc0 );
135 vg_line( ct
->co
, p1
, 0xffffffff );
140 * -----------------------------------------------------------------------------
142 * -----------------------------------------------------------------------------
146 * Update ALL matrices and tensors on rigidbody
148 static void rb_update_matrices( rigidbody
*rb
){
149 //q_normalize( rb->q );
150 q_m3x3( rb
->q
, rb
->to_world
);
151 v3_copy( rb
->co
, rb
->to_world
[3] );
152 m4x3_invert_affine( rb
->to_world
, rb
->to_local
);
155 m3x3_mul( rb
->to_world
, rb
->iI
, rb
->iIw
);
156 m3x3_mul( rb
->iIw
, rb
->to_local
, rb
->iIw
);
160 * Extrapolate rigidbody into a transform based on vg accumulator.
161 * Useful for rendering
163 static void rb_extrapolate( rigidbody
*rb
, v3f co
, v4f q
){
164 float substep
= vg
.time_fixed_extrapolate
;
165 v3_muladds( rb
->co
, rb
->v
, k_rb_delta
*substep
, co
);
167 if( v3_length2( rb
->w
) > 0.0f
){
170 v3_copy( rb
->w
, axis
);
172 float mag
= v3_length( axis
);
173 v3_divs( axis
, mag
, axis
);
174 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
*substep
);
175 q_mul( rotation
, rb
->q
, q
);
185 * -----------------------------------------------------------------------------
189 * Translate existing inertia tensor
191 static void rb_translate_inertia( m3x3f inout_inertia
, f32 mass
, v3f d
){
193 * I = I_0 + m*[(d.d)E_3 - d(X)d]
196 * I_0: original tensor
198 * d: translation vector
200 * E_3: identity matrix
202 m3x3f t
, outer
, scale
;
203 m3x3_diagonal( t
, v3_dot(d
,d
) );
204 m3x3_outer_product( outer
, d
, d
);
205 m3x3_sub( t
, outer
, t
);
206 m3x3_diagonal( scale
, mass
);
207 m3x3_mul( scale
, t
, t
);
208 m3x3_add( inout_inertia
, t
, inout_inertia
);
212 * Rotate existing inertia tensor
214 static void rb_rotate_inertia( m3x3f inout_inertia
, m3x3f rotation
){
219 * I_0: original tensor
221 * R^T: tranposed rotation matrix
225 m3x3_transpose( rotation
, Rt
);
226 m3x3_mul( rotation
, inout_inertia
, inout_inertia
);
227 m3x3_mul( inout_inertia
, Rt
, inout_inertia
);
230 * Create inertia tensor for box
232 static void rb_box_inertia( boxf box
, f32 mass
, m3x3f out_inertia
){
234 v3_sub( box
[1], box
[0], e
);
235 v3_muladds( box
[0], e
, 0.5f
, com
);
240 ix
= (ey2
+ez2
) * mass
* (1.0f
/12.0f
),
241 iy
= (ex2
+ez2
) * mass
* (1.0f
/12.0f
),
242 iz
= (ex2
+ey2
) * mass
* (1.0f
/12.0f
);
244 m3x3_identity( out_inertia
);
245 m3x3_setdiagonalv3( out_inertia
, (v3f
){ ix
, iy
, iz
} );
246 rb_translate_inertia( out_inertia
, mass
, com
);
250 * Create inertia tensor for sphere
252 static void rb_sphere_inertia( f32 r
, f32 mass
, m3x3f out_inertia
){
253 f32 ixyz
= r
*r
* mass
* (2.0f
/5.0f
);
255 m3x3_identity( out_inertia
);
256 m3x3_setdiagonalv3( out_inertia
, (v3f
){ ixyz
, ixyz
, ixyz
} );
260 * Create inertia tensor for capsule
264 static void rb_capsule_inertia( f32 r
, f32 h
, f32 mass
, m3x3f out_inertia
){
265 f32 density
= mass
/ vg_capsule_volume( r
, h
),
266 ch
= h
-r
*2.0f
, /* cylinder height */
267 cm
= VG_PIf
* ch
*r
*r
* density
, /* cylinder mass */
268 hm
= VG_TAUf
* (1.0f
/3.0f
) * r
*r
*r
* density
, /* hemisphere mass */
271 ixz
= iy
* 0.5f
+ cm
*ch
*ch
*(1.0f
/12.0f
),
273 aux0
= (hm
*2.0f
*r
*r
)/5.0f
;
278 aux2
= aux0
+ hm
*(aux1
*aux1
+ 3.0f
*(1.0f
/8.0f
)*ch
*r
);
282 m3x3_identity( out_inertia
);
283 m3x3_setdiagonalv3( out_inertia
, (v3f
){ ixz
, iy
, ixz
} );
286 static void rb_setbody_capsule( rigidbody
*rb
, f32 r
, f32 h
,
287 f32 density
, f32 inertia_scale
){
288 f32 vol
= vg_capsule_volume( r
, h
),
291 rb
->inv_mass
= 1.0f
/mass
;
294 rb_capsule_inertia( r
, h
, mass
* inertia_scale
, I
);
295 m3x3_inv( I
, rb
->iI
);
298 static void rb_setbody_box( rigidbody
*rb
, boxf box
,
299 f32 density
, f32 inertia_scale
){
300 f32 vol
= vg_box_volume( box
),
303 rb
->inv_mass
= 1.0f
/mass
;
306 rb_box_inertia( box
, mass
* inertia_scale
, I
);
307 m3x3_inv( I
, rb
->iI
);
310 static void rb_setbody_sphere( rigidbody
*rb
, f32 r
,
311 f32 density
, f32 inertia_scale
){
312 f32 vol
= vg_sphere_volume( r
),
315 rb
->inv_mass
= 1.0f
/mass
;
317 rb_sphere_inertia( r
, mass
* inertia_scale
, I
);
318 m3x3_inv( I
, rb
->iI
);
321 static void rb_iter( rigidbody
*rb
){
322 if( !vg_validf( rb
->v
[0] ) ||
323 !vg_validf( rb
->v
[1] ) ||
324 !vg_validf( rb
->v
[2] ) )
326 vg_fatal_error( "NaN velocity" );
329 v3f gravity
= { 0.0f
, -9.8f
, 0.0f
};
330 v3_muladds( rb
->v
, gravity
, k_rb_delta
, rb
->v
);
332 /* intergrate velocity */
333 v3_muladds( rb
->co
, rb
->v
, k_rb_delta
, rb
->co
);
334 v3_lerp( rb
->w
, (v3f
){0.0f
,0.0f
,0.0f
}, 0.0025f
, rb
->w
);
336 /* inegrate inertia */
337 if( v3_length2( rb
->w
) > 0.0f
){
340 v3_copy( rb
->w
, axis
);
342 float mag
= v3_length( axis
);
343 v3_divs( axis
, mag
, axis
);
344 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
);
345 q_mul( rotation
, rb
->q
, rb
->q
);
346 q_normalize( rb
->q
);
351 * -----------------------------------------------------------------------------
352 * Boolean shape overlap functions
353 * -----------------------------------------------------------------------------
357 * Project AABB, and triangle interval onto axis to check if they overlap
359 static int rb_box_triangle_interval( v3f extent
, v3f axis
, v3f tri
[3] ){
362 r
= extent
[0] * fabsf(axis
[0]) +
363 extent
[1] * fabsf(axis
[1]) +
364 extent
[2] * fabsf(axis
[2]),
366 p0
= v3_dot( axis
, tri
[0] ),
367 p1
= v3_dot( axis
, tri
[1] ),
368 p2
= v3_dot( axis
, tri
[2] ),
370 e
= vg_maxf(-vg_maxf(p0
,vg_maxf(p1
,p2
)), vg_minf(p0
,vg_minf(p1
,p2
)));
372 if( e
> r
) return 0;
377 * Seperating axis test box vs triangle
379 static int rb_box_triangle_sat( v3f extent
, v3f center
,
380 m4x3f to_local
, v3f tri_src
[3] ){
383 for( int i
=0; i
<3; i
++ ){
384 m4x3_mulv( to_local
, tri_src
[i
], tri
[i
] );
385 v3_sub( tri
[i
], center
, tri
[i
] );
389 v3_sub( tri
[1], tri
[0], f0
);
390 v3_sub( tri
[2], tri
[1], f1
);
391 v3_sub( tri
[0], tri
[2], f2
);
395 v3_cross( (v3f
){1.0f
,0.0f
,0.0f
}, f0
, axis
[0] );
396 v3_cross( (v3f
){1.0f
,0.0f
,0.0f
}, f1
, axis
[1] );
397 v3_cross( (v3f
){1.0f
,0.0f
,0.0f
}, f2
, axis
[2] );
398 v3_cross( (v3f
){0.0f
,1.0f
,0.0f
}, f0
, axis
[3] );
399 v3_cross( (v3f
){0.0f
,1.0f
,0.0f
}, f1
, axis
[4] );
400 v3_cross( (v3f
){0.0f
,1.0f
,0.0f
}, f2
, axis
[5] );
401 v3_cross( (v3f
){0.0f
,0.0f
,1.0f
}, f0
, axis
[6] );
402 v3_cross( (v3f
){0.0f
,0.0f
,1.0f
}, f1
, axis
[7] );
403 v3_cross( (v3f
){0.0f
,0.0f
,1.0f
}, f2
, axis
[8] );
405 for( int i
=0; i
<9; i
++ )
406 if(!rb_box_triangle_interval( extent
, axis
[i
], tri
)) return 0;
409 if(!rb_box_triangle_interval( extent
, (v3f
){1.0f
,0.0f
,0.0f
}, tri
)) return 0;
410 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,1.0f
,0.0f
}, tri
)) return 0;
411 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,0.0f
,1.0f
}, tri
)) return 0;
414 v3_cross( f0
, f1
, n
);
415 if(!rb_box_triangle_interval( extent
, n
, tri
)) return 0;
421 * -----------------------------------------------------------------------------
423 * -----------------------------------------------------------------------------
426 static int rb_manifold_apply_filtered( rb_ct
*man
, int len
){
429 for( int i
=0; i
<len
; i
++ ){
432 if( ct
->type
== k_contact_type_disabled
)
442 * Merge two contacts if they are within radius(r) of eachother
444 static void rb_manifold_contact_weld( rb_ct
*ci
, rb_ct
*cj
, float r
){
445 if( v3_dist2( ci
->co
, cj
->co
) < r
*r
){
446 cj
->type
= k_contact_type_disabled
;
447 ci
->p
= (ci
->p
+ cj
->p
) * 0.5f
;
449 v3_add( ci
->co
, cj
->co
, ci
->co
);
450 v3_muls( ci
->co
, 0.5f
, ci
->co
);
453 v3_sub( ci
->rba
->co
, ci
->co
, delta
);
455 float c0
= v3_dot( ci
->n
, delta
),
456 c1
= v3_dot( cj
->n
, delta
);
458 if( c0
< 0.0f
|| c1
< 0.0f
){
460 ci
->type
= k_contact_type_disabled
;
464 v3_muls( ci
->n
, c0
, n
);
465 v3_muladds( n
, cj
->n
, c1
, n
);
475 static void rb_manifold_filter_joint_edges( rb_ct
*man
, int len
, float r
){
476 for( int i
=0; i
<len
-1; i
++ ){
478 if( ci
->type
!= k_contact_type_edge
)
481 for( int j
=i
+1; j
<len
; j
++ ){
483 if( cj
->type
!= k_contact_type_edge
)
486 rb_manifold_contact_weld( ci
, cj
, r
);
492 * Resolve overlapping pairs
494 static void rb_manifold_filter_pairs( rb_ct
*man
, int len
, float r
){
495 for( int i
=0; i
<len
-1; i
++ ){
499 if( ci
->type
== k_contact_type_disabled
) continue;
501 for( int j
=i
+1; j
<len
; j
++ ){
504 if( cj
->type
== k_contact_type_disabled
) continue;
506 if( v3_dist2( ci
->co
, cj
->co
) < r
*r
){
507 cj
->type
= k_contact_type_disabled
;
508 v3_add( cj
->n
, ci
->n
, ci
->n
);
515 float n
= 1.0f
/((float)similar
+1.0f
);
516 v3_muls( ci
->n
, n
, ci
->n
);
519 if( v3_length2(ci
->n
) < 0.1f
*0.1f
)
520 ci
->type
= k_contact_type_disabled
;
522 v3_normalize( ci
->n
);
528 * Remove contacts that are facing away from A
530 static void rb_manifold_filter_backface( rb_ct
*man
, int len
){
531 for( int i
=0; i
<len
; i
++ ){
533 if( ct
->type
== k_contact_type_disabled
)
537 v3_sub( ct
->co
, ct
->rba
->co
, delta
);
539 if( v3_dot( delta
, ct
->n
) > -0.001f
)
540 ct
->type
= k_contact_type_disabled
;
545 * Filter out duplicate coplanar results. Good for spheres.
547 static void rb_manifold_filter_coplanar( rb_ct
*man
, int len
, float w
){
548 for( int i
=0; i
<len
; i
++ ){
550 if( ci
->type
== k_contact_type_disabled
||
551 ci
->type
== k_contact_type_edge
)
554 float d1
= v3_dot( ci
->co
, ci
->n
);
556 for( int j
=0; j
<len
; j
++ ){
561 if( cj
->type
== k_contact_type_disabled
)
564 float d2
= v3_dot( cj
->co
, ci
->n
),
567 if( fabsf( d
) <= w
){
568 cj
->type
= k_contact_type_disabled
;
575 * -----------------------------------------------------------------------------
577 * -----------------------------------------------------------------------------
583 * These do not automatically allocate contacts, an appropriately sized
584 * buffer must be supplied. The function returns the size of the manifold
585 * which was generated.
587 * The values set on the contacts are: n, co, p, rba, rbb
591 * By collecting the minimum(time) and maximum(time) pairs of points, we
592 * build a reduced and stable exact manifold.
595 * rx: minimum distance of these points
596 * dx: the delta between the two points
598 * pairs will only ammend these if they are creating a collision
600 typedef struct capsule_manifold capsule_manifold
;
601 struct capsule_manifold
{
608 * Expand a line manifold with a new pair. t value is the time along segment
609 * on the oriented object which created this pair.
611 static void rb_capsule_manifold( v3f pa
, v3f pb
, float t
, float r
,
612 capsule_manifold
*manifold
){
614 v3_sub( pa
, pb
, delta
);
616 if( v3_length2(delta
) < r
*r
){
617 if( t
< manifold
->t0
){
618 v3_copy( delta
, manifold
->d0
);
623 if( t
> manifold
->t1
){
624 v3_copy( delta
, manifold
->d1
);
631 static void rb_capsule_manifold_init( capsule_manifold
*manifold
){
632 manifold
->t0
= INFINITY
;
633 manifold
->t1
= -INFINITY
;
636 static int rb_capsule__manifold_done( m4x3f mtx
, rb_capsule
*c
,
637 capsule_manifold
*manifold
,
640 v3_muladds( mtx
[3], mtx
[1], -c
->h
*0.5f
+c
->r
, p0
);
641 v3_muladds( mtx
[3], mtx
[1], c
->h
*0.5f
-c
->r
, p1
);
644 if( manifold
->t0
<= 1.0f
){
648 v3_muls( p0
, 1.0f
-manifold
->t0
, pa
);
649 v3_muladds( pa
, p1
, manifold
->t0
, pa
);
651 float d
= v3_length( manifold
->d0
);
652 v3_muls( manifold
->d0
, 1.0f
/d
, ct
->n
);
653 v3_muladds( pa
, ct
->n
, -c
->r
, ct
->co
);
655 ct
->p
= manifold
->r0
- d
;
656 ct
->type
= k_contact_type_default
;
660 if( (manifold
->t1
>= 0.0f
) && (manifold
->t0
!= manifold
->t1
) ){
661 rb_ct
*ct
= buf
+count
;
664 v3_muls( p0
, 1.0f
-manifold
->t1
, pa
);
665 v3_muladds( pa
, p1
, manifold
->t1
, pa
);
667 float d
= v3_length( manifold
->d1
);
668 v3_muls( manifold
->d1
, 1.0f
/d
, ct
->n
);
669 v3_muladds( pa
, ct
->n
, -c
->r
, ct
->co
);
671 ct
->p
= manifold
->r1
- d
;
672 ct
->type
= k_contact_type_default
;
682 vg_line( buf
[0].co
, buf
[1].co
, 0xff0000ff );
688 static int rb_capsule_sphere( rb_object
*obja
, rb_object
*objb
, rb_ct
*buf
){
689 rigidbody
*rba
= &obja
->rb
, *rbb
= &objb
->rb
;
690 float h
= obja
->inf
.capsule
.h
,
691 ra
= obja
->inf
.capsule
.r
,
692 rb
= objb
->inf
.sphere
.r
;
695 v3_muladds( rba
->co
, rba
->to_world
[1], -h
*0.5f
+ra
, p0
);
696 v3_muladds( rba
->co
, rba
->to_world
[1], h
*0.5f
-ra
, p1
);
699 closest_point_segment( p0
, p1
, rbb
->co
, c
);
700 v3_sub( c
, rbb
->co
, delta
);
702 float d2
= v3_length2(delta
),
709 v3_muls( delta
, 1.0f
/d
, ct
->n
);
713 v3_muladds( c
, ct
->n
, -ra
, p0
);
714 v3_muladds( rbb
->co
, ct
->n
, rb
, p1
);
715 v3_add( p0
, p1
, ct
->co
);
716 v3_muls( ct
->co
, 0.5f
, ct
->co
);
720 ct
->type
= k_contact_type_default
;
729 static int rb_capsule__capsule( m4x3f mtxA
, rb_capsule
*ca
,
730 m4x3f mtxB
, rb_capsule
*cb
, rb_ct
*buf
){
738 v3_muladds( mtxA
[3], mtxA
[1], -ha
*0.5f
+ra
, p0
);
739 v3_muladds( mtxA
[3], mtxA
[1], ha
*0.5f
-ra
, p1
);
740 v3_muladds( mtxB
[3], mtxB
[1], -hb
*0.5f
+rb
, p2
);
741 v3_muladds( mtxB
[3], mtxB
[1], hb
*0.5f
-rb
, p3
);
743 capsule_manifold manifold
;
744 rb_capsule_manifold_init( &manifold
);
748 closest_segment_segment( p0
, p1
, p2
, p3
, &ta
, &tb
, pa
, pb
);
749 rb_capsule_manifold( pa
, pb
, ta
, r
, &manifold
);
751 ta
= closest_point_segment( p0
, p1
, p2
, pa
);
752 tb
= closest_point_segment( p0
, p1
, p3
, pb
);
753 rb_capsule_manifold( pa
, p2
, ta
, r
, &manifold
);
754 rb_capsule_manifold( pb
, p3
, tb
, r
, &manifold
);
756 closest_point_segment( p2
, p3
, p0
, pa
);
757 closest_point_segment( p2
, p3
, p1
, pb
);
758 rb_capsule_manifold( p0
, pa
, 0.0f
, r
, &manifold
);
759 rb_capsule_manifold( p1
, pb
, 1.0f
, r
, &manifold
);
761 return rb_capsule__manifold_done( mtxA
, ca
, &manifold
, buf
);
765 static int rb_sphere_box( rb_object
*obja
, rb_object
*objb
, rb_ct
*buf
){
767 rigidbody
*rba
= &obja
->rb
, *rbb
= &objb
->rb
;
769 closest_point_obb( rba
->co
, rbb
->bbx
, rbb
->to_world
, rbb
->to_local
, co
);
770 v3_sub( rba
->co
, co
, delta
);
772 float d2
= v3_length2(delta
),
773 r
= obja
->inf
.sphere
.radius
;
780 v3_sub( rba
->co
, rbb
->co
, delta
);
783 * some extra testing is required to find the best axis to push the
784 * object back outside the box. Since there isnt a clear seperating
785 * vector already, especially on really high aspect boxes.
787 float lx
= v3_dot( rbb
->to_world
[0], delta
),
788 ly
= v3_dot( rbb
->to_world
[1], delta
),
789 lz
= v3_dot( rbb
->to_world
[2], delta
),
790 px
= rbb
->bbx
[1][0] - fabsf(lx
),
791 py
= rbb
->bbx
[1][1] - fabsf(ly
),
792 pz
= rbb
->bbx
[1][2] - fabsf(lz
);
794 if( px
< py
&& px
< pz
)
795 v3_muls( rbb
->to_world
[0], vg_signf(lx
), ct
->n
);
797 v3_muls( rbb
->to_world
[1], vg_signf(ly
), ct
->n
);
799 v3_muls( rbb
->to_world
[2], vg_signf(lz
), ct
->n
);
801 v3_muladds( rba
->co
, ct
->n
, -r
, ct
->co
);
806 v3_muls( delta
, 1.0f
/d
, ct
->n
);
808 v3_copy( co
, ct
->co
);
813 ct
->type
= k_contact_type_default
;
822 static int rb_sphere_sphere( rb_object
*obja
, rb_object
*objb
, rb_ct
*buf
){
823 rigidbody
*rba
= &obja
->rb
, *rbb
= &objb
->rb
;
825 v3_sub( rba
->co
, rbb
->co
, delta
);
827 float d2
= v3_length2(delta
),
828 r
= obja
->inf
.sphere
.radius
+ objb
->inf
.sphere
.radius
;
834 v3_muls( delta
, 1.0f
/d
, ct
->n
);
837 v3_muladds( rba
->co
, ct
->n
,-obja
->inf
.sphere
.radius
, p0
);
838 v3_muladds( rbb
->co
, ct
->n
, objb
->inf
.sphere
.radius
, p1
);
839 v3_add( p0
, p1
, ct
->co
);
840 v3_muls( ct
->co
, 0.5f
, ct
->co
);
841 ct
->type
= k_contact_type_default
;
852 static int rb_sphere__triangle( m4x3f mtxA
, f32 r
,
853 v3f tri
[3], rb_ct
*buf
){
855 enum contact_type type
= closest_on_triangle_1( mtxA
[3], tri
, co
);
856 v3_sub( mtxA
[3], co
, delta
);
857 f32 d2
= v3_length2( delta
);
863 v3_sub( tri
[2], tri
[0], ab
);
864 v3_sub( tri
[1], tri
[0], ac
);
865 v3_cross( ac
, ab
, tn
);
866 v3_copy( tn
, ct
->n
);
868 if( v3_length2( ct
->n
) <= 0.00001f
){
869 #ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING
870 vg_error( "Zero area triangle!\n" );
875 v3_normalize( ct
->n
);
879 v3_copy( co
, ct
->co
);
888 static int rb_sphere__scene( m4x3f mtxA
, f32 r
,
889 m4x3f mtxB
, bh_tree
*scene_bh
, rb_ct
*buf
,
891 scene_context
*sc
= scene_bh
->user
;
896 v3_sub( mtxA
[3], (v3f
){ r
,r
,r
}, box
[0] );
897 v3_add( mtxA
[3], (v3f
){ r
,r
,r
}, box
[1] );
901 bh_iter_init_box( 0, &it
, box
);
903 while( bh_next( scene_bh
, &it
, &idx
) ){
904 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
907 if( sc
->arrvertices
[ptri
[0]].flags
& ignore
) continue;
909 for( int j
=0; j
<3; j
++ )
910 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
912 buf
[ count
].element_id
= ptri
[0];
914 vg_line( tri
[0],tri
[1],0x70ff6000 );
915 vg_line( tri
[1],tri
[2],0x70ff6000 );
916 vg_line( tri
[2],tri
[0],0x70ff6000 );
918 int contact
= rb_sphere__triangle( mtxA
, r
, tri
, &buf
[count
] );
922 vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
930 static int rb_box__scene( m4x3f mtxA
, boxf bbx
,
931 m4x3f mtxB
, bh_tree
*scene_bh
,
932 rb_ct
*buf
, u16 ignore
){
933 scene_context
*sc
= scene_bh
->user
;
937 v3_sub( bbx
[1], bbx
[0], extent
);
938 v3_muls( extent
, 0.5f
, extent
);
939 v3_add( bbx
[0], extent
, center
);
941 float r
= v3_length(extent
);
943 v3_fill( world_bbx
[0], -r
);
944 v3_fill( world_bbx
[1], r
);
945 for( int i
=0; i
<2; i
++ ){
946 v3_add( center
, world_bbx
[i
], world_bbx
[i
] );
947 v3_add( mtxA
[3], world_bbx
[i
], world_bbx
[i
] );
951 m4x3_invert_affine( mtxA
, to_local
);
954 bh_iter_init_box( 0, &it
, world_bbx
);
958 vg_line_boxf( world_bbx
, VG__RED
);
960 while( bh_next( scene_bh
, &it
, &idx
) ){
961 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
962 if( sc
->arrvertices
[ptri
[0]].flags
& ignore
) continue;
964 for( int j
=0; j
<3; j
++ )
965 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
967 if( rb_box_triangle_sat( extent
, center
, to_local
, tri
) ){
968 vg_line(tri
[0],tri
[1],0xff50ff00 );
969 vg_line(tri
[1],tri
[2],0xff50ff00 );
970 vg_line(tri
[2],tri
[0],0xff50ff00 );
973 vg_line(tri
[0],tri
[1],0xff0000ff );
974 vg_line(tri
[1],tri
[2],0xff0000ff );
975 vg_line(tri
[2],tri
[0],0xff0000ff );
980 v3_sub( tri
[1], tri
[0], v0
);
981 v3_sub( tri
[2], tri
[0], v1
);
982 v3_cross( v0
, v1
, n
);
984 if( v3_length2( n
) <= 0.00001f
){
985 #ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING
986 vg_error( "Zero area triangle!\n" );
993 /* find best feature */
994 float best
= v3_dot( mtxA
[0], n
);
997 for( int i
=1; i
<3; i
++ ){
998 float c
= v3_dot( mtxA
[i
], n
);
1000 if( fabsf(c
) > fabsf(best
) ){
1009 float px
= best
> 0.0f
? bbx
[0][0]: bbx
[1][0];
1010 manifold
[0][0] = px
;
1011 manifold
[0][1] = bbx
[0][1];
1012 manifold
[0][2] = bbx
[0][2];
1013 manifold
[1][0] = px
;
1014 manifold
[1][1] = bbx
[1][1];
1015 manifold
[1][2] = bbx
[0][2];
1016 manifold
[2][0] = px
;
1017 manifold
[2][1] = bbx
[1][1];
1018 manifold
[2][2] = bbx
[1][2];
1019 manifold
[3][0] = px
;
1020 manifold
[3][1] = bbx
[0][1];
1021 manifold
[3][2] = bbx
[1][2];
1023 else if( axis
== 1 ){
1024 float py
= best
> 0.0f
? bbx
[0][1]: bbx
[1][1];
1025 manifold
[0][0] = bbx
[0][0];
1026 manifold
[0][1] = py
;
1027 manifold
[0][2] = bbx
[0][2];
1028 manifold
[1][0] = bbx
[1][0];
1029 manifold
[1][1] = py
;
1030 manifold
[1][2] = bbx
[0][2];
1031 manifold
[2][0] = bbx
[1][0];
1032 manifold
[2][1] = py
;
1033 manifold
[2][2] = bbx
[1][2];
1034 manifold
[3][0] = bbx
[0][0];
1035 manifold
[3][1] = py
;
1036 manifold
[3][2] = bbx
[1][2];
1039 float pz
= best
> 0.0f
? bbx
[0][2]: bbx
[1][2];
1040 manifold
[0][0] = bbx
[0][0];
1041 manifold
[0][1] = bbx
[0][1];
1042 manifold
[0][2] = pz
;
1043 manifold
[1][0] = bbx
[1][0];
1044 manifold
[1][1] = bbx
[0][1];
1045 manifold
[1][2] = pz
;
1046 manifold
[2][0] = bbx
[1][0];
1047 manifold
[2][1] = bbx
[1][1];
1048 manifold
[2][2] = pz
;
1049 manifold
[3][0] = bbx
[0][0];
1050 manifold
[3][1] = bbx
[1][1];
1051 manifold
[3][2] = pz
;
1054 for( int j
=0; j
<4; j
++ )
1055 m4x3_mulv( mtxA
, manifold
[j
], manifold
[j
] );
1057 vg_line( manifold
[0], manifold
[1], 0xffffffff );
1058 vg_line( manifold
[1], manifold
[2], 0xffffffff );
1059 vg_line( manifold
[2], manifold
[3], 0xffffffff );
1060 vg_line( manifold
[3], manifold
[0], 0xffffffff );
1062 for( int j
=0; j
<4; j
++ ){
1063 rb_ct
*ct
= buf
+count
;
1065 v3_copy( manifold
[j
], ct
->co
);
1066 v3_copy( n
, ct
->n
);
1068 float l0
= v3_dot( tri
[0], n
),
1069 l1
= v3_dot( manifold
[j
], n
);
1071 ct
->p
= (l0
-l1
)*0.5f
;
1075 ct
->type
= k_contact_type_default
;
1085 static int rb_capsule__triangle( m4x3f mtxA
, rb_capsule
*c
,
1086 v3f tri
[3], rb_ct
*buf
){
1088 v3_muladds( mtxA
[3], mtxA
[1], -c
->h
*0.5f
+c
->r
, p0w
);
1089 v3_muladds( mtxA
[3], mtxA
[1], c
->h
*0.5f
-c
->r
, p1w
);
1091 capsule_manifold manifold
;
1092 rb_capsule_manifold_init( &manifold
);
1095 v3_sub( tri
[1], tri
[0], v0
);
1096 v3_sub( tri
[2], tri
[0], v1
);
1097 v3_cross( v0
, v1
, n
);
1099 if( v3_length2( n
) <= 0.00001f
){
1100 #ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING
1101 vg_error( "Zero area triangle!\n" );
1109 /* deep penetration recovery. for when we clip through the triangles. so its
1110 * not very 'correct' */
1112 if( ray_tri( tri
, p0w
, mtxA
[1], &dist
, 1 ) ){
1113 f32 l
= c
->h
- c
->r
*2.0f
;
1114 if( (dist
>= 0.0f
) && (dist
< l
) ){
1116 v3_muladds( p0w
, mtxA
[1], dist
, co
);
1117 vg_line_point( co
, 0.02f
, 0xffffff00 );
1120 v3_sub( p0w
, co
, d0
);
1121 v3_sub( p1w
, co
, d1
);
1123 f32 p
= vg_minf( v3_dot( n
, d0
), v3_dot( n
, d1
) ) - c
->r
;
1127 ct
->type
= k_contact_type_default
;
1128 v3_copy( n
, ct
->n
);
1129 v3_muladds( co
, n
, p
, ct
->co
);
1137 closest_on_triangle_1( p0w
, tri
, c0
);
1138 closest_on_triangle_1( p1w
, tri
, c1
);
1141 v3_sub( c0
, p0w
, d0
);
1142 v3_sub( c1
, p1w
, d1
);
1143 v3_sub( p1w
, p0w
, da
);
1149 /* the two balls at the ends */
1150 if( v3_dot( da
, d0
) <= 0.01f
)
1151 rb_capsule_manifold( p0w
, c0
, 0.0f
, c
->r
, &manifold
);
1152 if( v3_dot( da
, d1
) >= -0.01f
)
1153 rb_capsule_manifold( p1w
, c1
, 1.0f
, c
->r
, &manifold
);
1155 /* the edges to edges */
1156 for( int i
=0; i
<3; i
++ ){
1162 closest_segment_segment( p0w
, p1w
, tri
[i0
], tri
[i1
], &ta
, &tb
, ca
, cb
);
1163 rb_capsule_manifold( ca
, cb
, ta
, c
->r
, &manifold
);
1166 int count
= rb_capsule__manifold_done( mtxA
, c
, &manifold
, buf
);
1167 for( int i
=0; i
<count
; i
++ )
1168 v3_copy( n
, buf
[i
].n
);
1173 /* mtxB is defined only for tradition; it is not used currently */
1174 static int rb_capsule__scene( m4x3f mtxA
, rb_capsule
*c
,
1175 m4x3f mtxB
, bh_tree
*scene_bh
,
1176 rb_ct
*buf
, u16 ignore
){
1180 v3_sub( mtxA
[3], (v3f
){ c
->h
, c
->h
, c
->h
}, bbx
[0] );
1181 v3_add( mtxA
[3], (v3f
){ c
->h
, c
->h
, c
->h
}, bbx
[1] );
1183 scene_context
*sc
= scene_bh
->user
;
1186 bh_iter_init_box( 0, &it
, bbx
);
1188 while( bh_next( scene_bh
, &it
, &idx
) ){
1189 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
1190 if( sc
->arrvertices
[ptri
[0]].flags
& ignore
) continue;
1193 for( int j
=0; j
<3; j
++ )
1194 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
1196 buf
[ count
].element_id
= ptri
[0];
1198 int contact
= rb_capsule__triangle( mtxA
, c
, tri
, &buf
[count
] );
1202 vg_warn("Exceeding capsule_vs_scene capacity. Geometry too dense!\n");
1210 static int rb_global_has_space( void ){
1211 if( rb_contact_count
+ 16 > vg_list_size(rb_contact_buffer
) )
1217 static rb_ct
*rb_global_buffer( void ){
1218 return &rb_contact_buffer
[ rb_contact_count
];
1222 * -----------------------------------------------------------------------------
1224 * -----------------------------------------------------------------------------
1227 static void rb_solver_reset(void){
1228 rb_contact_count
= 0;
1231 static rb_ct
*rb_global_ct(void){
1232 return rb_contact_buffer
+ rb_contact_count
;
1235 static void rb_prepare_contact( rb_ct
*ct
, float timestep
){
1236 ct
->bias
= -k_phys_baumgarte
* (timestep
*3600.0f
)
1237 * vg_minf( 0.0f
, -ct
->p
+k_penetration_slop
);
1239 v3_tangent_basis( ct
->n
, ct
->t
[0], ct
->t
[1] );
1240 ct
->norm_impulse
= 0.0f
;
1241 ct
->tangent_impulse
[0] = 0.0f
;
1242 ct
->tangent_impulse
[1] = 0.0f
;
1245 /* calculate total move. manifold should belong to ONE object only */
1246 static void rb_depenetrate( rb_ct
*manifold
, int len
, v3f dt
){
1249 for( int j
=0; j
<7; j
++ )
1251 for( int i
=0; i
<len
; i
++ )
1253 struct contact
*ct
= &manifold
[i
];
1255 float resolved_amt
= v3_dot( ct
->n
, dt
),
1256 remaining
= (ct
->p
-k_penetration_slop
) - resolved_amt
,
1257 apply
= vg_maxf( remaining
, 0.0f
) * 0.4f
;
1259 v3_muladds( dt
, ct
->n
, apply
, dt
);
1265 * Initializing things like tangent vectors
1267 static void rb_presolve_contacts( rb_ct
*buffer
, int len
){
1268 for( int i
=0; i
<len
; i
++ ){
1269 rb_ct
*ct
= &buffer
[i
];
1270 rb_prepare_contact( ct
, k_rb_delta
);
1272 v3f ra
, rb
, raCn
, rbCn
, raCt
, rbCt
;
1273 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
1274 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
1275 v3_cross( ra
, ct
->n
, raCn
);
1276 v3_cross( rb
, ct
->n
, rbCn
);
1278 /* orient inverse inertia tensors */
1280 m3x3_mulv( ct
->rba
->iIw
, raCn
, raCnI
);
1281 m3x3_mulv( ct
->rbb
->iIw
, rbCn
, rbCnI
);
1283 ct
->normal_mass
= ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
1284 ct
->normal_mass
+= v3_dot( raCn
, raCnI
);
1285 ct
->normal_mass
+= v3_dot( rbCn
, rbCnI
);
1286 ct
->normal_mass
= 1.0f
/ct
->normal_mass
;
1288 for( int j
=0; j
<2; j
++ ){
1290 v3_cross( ct
->t
[j
], ra
, raCt
);
1291 v3_cross( ct
->t
[j
], rb
, rbCt
);
1292 m3x3_mulv( ct
->rba
->iIw
, raCt
, raCtI
);
1293 m3x3_mulv( ct
->rbb
->iIw
, rbCt
, rbCtI
);
1295 ct
->tangent_mass
[j
] = ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
1296 ct
->tangent_mass
[j
] += v3_dot( raCt
, raCtI
);
1297 ct
->tangent_mass
[j
] += v3_dot( rbCt
, rbCtI
);
1298 ct
->tangent_mass
[j
] = 1.0f
/ct
->tangent_mass
[j
];
1301 rb_debug_contact( ct
);
1306 * Creates relative contact velocity vector
1308 static void rb_rcv( rigidbody
*rba
, rigidbody
*rbb
, v3f ra
, v3f rb
, v3f rv
){
1310 v3_cross( rba
->w
, ra
, rva
);
1311 v3_add( rba
->v
, rva
, rva
);
1312 v3_cross( rbb
->w
, rb
, rvb
);
1313 v3_add( rbb
->v
, rvb
, rvb
);
1315 v3_sub( rva
, rvb
, rv
);
1318 static void rb_contact_restitution( rb_ct
*ct
, float cr
){
1320 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
1321 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
1322 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
1324 float v
= v3_dot( rv
, ct
->n
);
1327 ct
->bias
+= -cr
* v
;
1332 * Apply impulse to object
1334 static void rb_linear_impulse( rigidbody
*rb
, v3f delta
, v3f impulse
){
1336 v3_muladds( rb
->v
, impulse
, rb
->inv_mass
, rb
->v
);
1338 /* Angular velocity */
1340 v3_cross( delta
, impulse
, wa
);
1342 m3x3_mulv( rb
->iIw
, wa
, wa
);
1343 v3_add( rb
->w
, wa
, rb
->w
);
1347 * One iteration to solve the contact constraint
1349 static void rb_solve_contacts( rb_ct
*buf
, int len
){
1350 for( int i
=0; i
<len
; i
++ ){
1351 struct contact
*ct
= &buf
[i
];
1354 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
1355 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
1356 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
1359 for( int j
=0; j
<2; j
++ ){
1360 float f
= k_friction
* ct
->norm_impulse
,
1361 vt
= v3_dot( rv
, ct
->t
[j
] ),
1362 lambda
= ct
->tangent_mass
[j
] * -vt
;
1364 float temp
= ct
->tangent_impulse
[j
];
1365 ct
->tangent_impulse
[j
] = vg_clampf( temp
+ lambda
, -f
, f
);
1366 lambda
= ct
->tangent_impulse
[j
] - temp
;
1369 v3_muls( ct
->t
[j
], lambda
, impulse
);
1370 rb_linear_impulse( ct
->rba
, ra
, impulse
);
1372 v3_muls( ct
->t
[j
], -lambda
, impulse
);
1373 rb_linear_impulse( ct
->rbb
, rb
, impulse
);
1377 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
1378 float vn
= v3_dot( rv
, ct
->n
),
1379 lambda
= ct
->normal_mass
* (-vn
+ ct
->bias
);
1381 float temp
= ct
->norm_impulse
;
1382 ct
->norm_impulse
= vg_maxf( temp
+ lambda
, 0.0f
);
1383 lambda
= ct
->norm_impulse
- temp
;
1386 v3_muls( ct
->n
, lambda
, impulse
);
1387 rb_linear_impulse( ct
->rba
, ra
, impulse
);
1389 v3_muls( ct
->n
, -lambda
, impulse
);
1390 rb_linear_impulse( ct
->rbb
, rb
, impulse
);
1395 * -----------------------------------------------------------------------------
1397 * -----------------------------------------------------------------------------
1400 static void rb_debug_position_constraints( rb_constr_pos
*buffer
, int len
){
1401 for( int i
=0; i
<len
; i
++ ){
1402 rb_constr_pos
*constr
= &buffer
[i
];
1403 rigidbody
*rba
= constr
->rba
, *rbb
= constr
->rbb
;
1406 m3x3_mulv( rba
->to_world
, constr
->lca
, wca
);
1407 m3x3_mulv( rbb
->to_world
, constr
->lcb
, wcb
);
1410 v3_add( wca
, rba
->co
, p0
);
1411 v3_add( wcb
, rbb
->co
, p1
);
1412 vg_line_point( p0
, 0.0025f
, 0xff000000 );
1413 vg_line_point( p1
, 0.0025f
, 0xffffffff );
1414 vg_line2( p0
, p1
, 0xff000000, 0xffffffff );
1418 static void rb_presolve_swingtwist_constraints( rb_constr_swingtwist
*buf
,
1420 for( int i
=0; i
<len
; i
++ ){
1421 rb_constr_swingtwist
*st
= &buf
[ i
];
1423 v3f vx
, vy
, va
, vxb
, axis
, center
;
1425 m3x3_mulv( st
->rba
->to_world
, st
->conevx
, vx
);
1426 m3x3_mulv( st
->rbb
->to_world
, st
->conevxb
, vxb
);
1427 m3x3_mulv( st
->rba
->to_world
, st
->conevy
, vy
);
1428 m3x3_mulv( st
->rbb
->to_world
, st
->coneva
, va
);
1429 m4x3_mulv( st
->rba
->to_world
, st
->view_offset
, center
);
1430 v3_cross( vy
, vx
, axis
);
1432 /* Constraint violated ? */
1433 float fx
= v3_dot( vx
, va
), /* projection world */
1434 fy
= v3_dot( vy
, va
),
1435 fn
= v3_dot( va
, axis
),
1437 rx
= st
->conevx
[3], /* elipse radii */
1440 lx
= fx
/rx
, /* projection local (fn==lz) */
1443 st
->tangent_violation
= ((lx
*lx
+ ly
*ly
) > fn
*fn
) || (fn
<= 0.0f
);
1444 if( st
->tangent_violation
){
1445 /* Calculate a good position and the axis to solve on */
1446 v2f closest
, tangent
,
1447 p
= { fx
/fabsf(fn
), fy
/fabsf(fn
) };
1449 closest_point_elipse( p
, (v2f
){rx
,ry
}, closest
);
1450 tangent
[0] = -closest
[1] / (ry
*ry
);
1451 tangent
[1] = closest
[0] / (rx
*rx
);
1452 v2_normalize( tangent
);
1455 v3_muladds( axis
, vx
, closest
[0], v0
);
1456 v3_muladds( v0
, vy
, closest
[1], v0
);
1459 v3_muls( vx
, tangent
[0], v1
);
1460 v3_muladds( v1
, vy
, tangent
[1], v1
);
1462 v3_copy( v0
, st
->tangent_target
);
1463 v3_copy( v1
, st
->tangent_axis
);
1465 /* calculate mass */
1467 m3x3_mulv( st
->rba
->iIw
, st
->tangent_axis
, aIw
);
1468 m3x3_mulv( st
->rbb
->iIw
, st
->tangent_axis
, bIw
);
1469 st
->tangent_mass
= 1.0f
/ (v3_dot( st
->tangent_axis
, aIw
) +
1470 v3_dot( st
->tangent_axis
, bIw
));
1472 float angle
= v3_dot( va
, st
->tangent_target
);
1476 v3_cross( vy
, va
, refaxis
); /* our default rotation */
1477 v3_normalize( refaxis
);
1479 float angle
= v3_dot( refaxis
, vxb
);
1480 st
->axis_violation
= fabsf(angle
) < st
->conet
;
1482 if( st
->axis_violation
){
1484 v3_cross( refaxis
, vxb
, dir_test
);
1486 if( v3_dot(dir_test
, va
) < 0.0f
)
1487 st
->axis_violation
= -st
->axis_violation
;
1489 float newang
= (float)st
->axis_violation
* acosf(st
->conet
-0.0001f
);
1492 v3_cross( va
, refaxis
, refaxis_up
);
1493 v3_muls( refaxis_up
, sinf(newang
), st
->axis_target
);
1494 v3_muladds( st
->axis_target
, refaxis
, -cosf(newang
), st
->axis_target
);
1496 /* calculate mass */
1497 v3_copy( va
, st
->axis
);
1499 m3x3_mulv( st
->rba
->iIw
, st
->axis
, aIw
);
1500 m3x3_mulv( st
->rbb
->iIw
, st
->axis
, bIw
);
1501 st
->axis_mass
= 1.0f
/ (v3_dot( st
->axis
, aIw
) +
1502 v3_dot( st
->axis
, bIw
));
1507 static void rb_debug_swingtwist_constraints( rb_constr_swingtwist
*buf
,
1511 for( int i
=0; i
<len
; i
++ ){
1512 rb_constr_swingtwist
*st
= &buf
[ i
];
1514 v3f vx
, vxb
, vy
, va
, axis
, center
;
1516 m3x3_mulv( st
->rba
->to_world
, st
->conevx
, vx
);
1517 m3x3_mulv( st
->rbb
->to_world
, st
->conevxb
, vxb
);
1518 m3x3_mulv( st
->rba
->to_world
, st
->conevy
, vy
);
1519 m3x3_mulv( st
->rbb
->to_world
, st
->coneva
, va
);
1520 m4x3_mulv( st
->rba
->to_world
, st
->view_offset
, center
);
1521 v3_cross( vy
, vx
, axis
);
1523 float rx
= st
->conevx
[3], /* elipse radii */
1527 v3_muladds( center
, va
, size
, p1
);
1528 vg_line( center
, p1
, 0xffffffff );
1529 vg_line_point( p1
, 0.00025f
, 0xffffffff );
1531 if( st
->tangent_violation
){
1532 v3_muladds( center
, st
->tangent_target
, size
, p0
);
1534 vg_line( center
, p0
, 0xff00ff00 );
1535 vg_line_point( p0
, 0.00025f
, 0xff00ff00 );
1536 vg_line( p1
, p0
, 0xff000000 );
1539 for( int x
=0; x
<32; x
++ ){
1540 float t0
= ((float)x
* (1.0f
/32.0f
)) * VG_TAUf
,
1541 t1
= (((float)x
+1.0f
) * (1.0f
/32.0f
)) * VG_TAUf
,
1548 v3_muladds( axis
, vx
, c0
*rx
, v0
);
1549 v3_muladds( v0
, vy
, s0
*ry
, v0
);
1550 v3_muladds( axis
, vx
, c1
*rx
, v1
);
1551 v3_muladds( v1
, vy
, s1
*ry
, v1
);
1556 v3_muladds( center
, v0
, size
, p0
);
1557 v3_muladds( center
, v1
, size
, p1
);
1559 u32 col0r
= fabsf(c0
) * 255.0f
,
1560 col0g
= fabsf(s0
) * 255.0f
,
1561 col1r
= fabsf(c1
) * 255.0f
,
1562 col1g
= fabsf(s1
) * 255.0f
,
1563 col
= st
->tangent_violation
? 0xff0000ff: 0xff000000,
1564 col0
= col
| (col0r
<<16) | (col0g
<< 8),
1565 col1
= col
| (col1r
<<16) | (col1g
<< 8);
1567 vg_line2( center
, p0
, VG__NONE
, col0
);
1568 vg_line2( p0
, p1
, col0
, col1
);
1572 v3_muladds( center
, va
, size
, p0
);
1573 v3_muladds( p0
, vxb
, size
, p1
);
1575 vg_line( p0
, p1
, 0xff0000ff );
1577 if( st
->axis_violation
){
1578 v3_muladds( p0
, st
->axis_target
, size
*1.25f
, p1
);
1579 vg_line( p0
, p1
, 0xffffff00 );
1580 vg_line_point( p1
, 0.0025f
, 0xffffff80 );
1584 v3_cross( vy
, va
, refaxis
); /* our default rotation */
1585 v3_normalize( refaxis
);
1587 v3_cross( va
, refaxis
, refaxis_up
);
1588 float newang
= acosf(st
->conet
-0.0001f
);
1590 v3_muladds( p0
, refaxis_up
, sinf(newang
)*size
, p1
);
1591 v3_muladds( p1
, refaxis
, -cosf(newang
)*size
, p1
);
1592 vg_line( p0
, p1
, 0xff000000 );
1594 v3_muladds( p0
, refaxis_up
, sinf(-newang
)*size
, p1
);
1595 v3_muladds( p1
, refaxis
, -cosf(-newang
)*size
, p1
);
1596 vg_line( p0
, p1
, 0xff404040 );
1601 * Solve a list of positional constraints
1603 static void rb_solve_position_constraints( rb_constr_pos
*buf
, int len
){
1604 for( int i
=0; i
<len
; i
++ ){
1605 rb_constr_pos
*constr
= &buf
[i
];
1606 rigidbody
*rba
= constr
->rba
, *rbb
= constr
->rbb
;
1609 m3x3_mulv( rba
->to_world
, constr
->lca
, wa
);
1610 m3x3_mulv( rbb
->to_world
, constr
->lcb
, wb
);
1612 m3x3f ssra
, ssrat
, ssrb
, ssrbt
;
1614 m3x3_skew_symetric( ssrat
, wa
);
1615 m3x3_skew_symetric( ssrbt
, wb
);
1616 m3x3_transpose( ssrat
, ssra
);
1617 m3x3_transpose( ssrbt
, ssrb
);
1619 v3f b
, b_wa
, b_wb
, b_a
, b_b
;
1620 m3x3_mulv( ssra
, rba
->w
, b_wa
);
1621 m3x3_mulv( ssrb
, rbb
->w
, b_wb
);
1622 v3_add( rba
->v
, b_wa
, b
);
1623 v3_sub( b
, rbb
->v
, b
);
1624 v3_sub( b
, b_wb
, b
);
1625 v3_muls( b
, -1.0f
, b
);
1628 m3x3_diagonal( invMa
, rba
->inv_mass
);
1629 m3x3_diagonal( invMb
, rbb
->inv_mass
);
1632 m3x3_mul( ssra
, rba
->iIw
, ia
);
1633 m3x3_mul( ia
, ssrat
, ia
);
1634 m3x3_mul( ssrb
, rbb
->iIw
, ib
);
1635 m3x3_mul( ib
, ssrbt
, ib
);
1638 m3x3_add( invMa
, ia
, cma
);
1639 m3x3_add( invMb
, ib
, cmb
);
1642 m3x3_add( cma
, cmb
, A
);
1644 /* Solve Ax = b ( A^-1*b = x ) */
1647 m3x3_inv( A
, invA
);
1648 m3x3_mulv( invA
, b
, impulse
);
1650 v3f delta_va
, delta_wa
, delta_vb
, delta_wb
;
1652 m3x3_mul( rba
->iIw
, ssrat
, iwa
);
1653 m3x3_mul( rbb
->iIw
, ssrbt
, iwb
);
1655 m3x3_mulv( invMa
, impulse
, delta_va
);
1656 m3x3_mulv( invMb
, impulse
, delta_vb
);
1657 m3x3_mulv( iwa
, impulse
, delta_wa
);
1658 m3x3_mulv( iwb
, impulse
, delta_wb
);
1660 v3_add( rba
->v
, delta_va
, rba
->v
);
1661 v3_add( rba
->w
, delta_wa
, rba
->w
);
1662 v3_sub( rbb
->v
, delta_vb
, rbb
->v
);
1663 v3_sub( rbb
->w
, delta_wb
, rbb
->w
);
1667 static void rb_solve_swingtwist_constraints( rb_constr_swingtwist
*buf
,
1669 for( int i
=0; i
<len
; i
++ ){
1670 rb_constr_swingtwist
*st
= &buf
[ i
];
1672 if( !st
->axis_violation
)
1675 float rv
= v3_dot( st
->axis
, st
->rbb
->w
) -
1676 v3_dot( st
->axis
, st
->rba
->w
);
1678 if( rv
* (float)st
->axis_violation
> 0.0f
)
1681 v3f impulse
, wa
, wb
;
1682 v3_muls( st
->axis
, rv
*st
->axis_mass
, impulse
);
1683 m3x3_mulv( st
->rba
->iIw
, impulse
, wa
);
1684 v3_add( st
->rba
->w
, wa
, st
->rba
->w
);
1686 v3_muls( impulse
, -1.0f
, impulse
);
1687 m3x3_mulv( st
->rbb
->iIw
, impulse
, wb
);
1688 v3_add( st
->rbb
->w
, wb
, st
->rbb
->w
);
1690 float rv2
= v3_dot( st
->axis
, st
->rbb
->w
) -
1691 v3_dot( st
->axis
, st
->rba
->w
);
1694 for( int i
=0; i
<len
; i
++ ){
1695 rb_constr_swingtwist
*st
= &buf
[ i
];
1697 if( !st
->tangent_violation
)
1700 float rv
= v3_dot( st
->tangent_axis
, st
->rbb
->w
) -
1701 v3_dot( st
->tangent_axis
, st
->rba
->w
);
1706 v3f impulse
, wa
, wb
;
1707 v3_muls( st
->tangent_axis
, rv
*st
->tangent_mass
, impulse
);
1708 m3x3_mulv( st
->rba
->iIw
, impulse
, wa
);
1709 v3_add( st
->rba
->w
, wa
, st
->rba
->w
);
1711 v3_muls( impulse
, -1.0f
, impulse
);
1712 m3x3_mulv( st
->rbb
->iIw
, impulse
, wb
);
1713 v3_add( st
->rbb
->w
, wb
, st
->rbb
->w
);
1715 float rv2
= v3_dot( st
->tangent_axis
, st
->rbb
->w
) -
1716 v3_dot( st
->tangent_axis
, st
->rba
->w
);
1721 static void rb_postsolve_swingtwist_constraints( rb_constr_swingtwist
*buf
,
1723 for( int i
=0; i
<len
; i
++ ){
1724 rb_constr_swingtwist
*st
= &buf
[ i
];
1726 if( !st
->axis_violation
){
1727 st
->conv_axis
= 0.0f
;
1731 f32 rv
= v3_dot( st
->axis
, st
->rbb
->w
) -
1732 v3_dot( st
->axis
, st
->rba
->w
);
1734 if( rv
* (f32
)st
->axis_violation
> 0.0f
)
1735 st
->conv_axis
= 0.0f
;
1740 for( int i
=0; i
<len
; i
++ ){
1741 rb_constr_swingtwist
*st
= &buf
[ i
];
1743 if( !st
->tangent_violation
){
1744 st
->conv_tangent
= 0.0f
;
1748 f32 rv
= v3_dot( st
->tangent_axis
, st
->rbb
->w
) -
1749 v3_dot( st
->tangent_axis
, st
->rba
->w
);
1752 st
->conv_tangent
= 0.0f
;
1754 st
->conv_tangent
= rv
;
1758 static void rb_solve_constr_angle( rigidbody
*rba
, rigidbody
*rbb
,
1760 m3x3f ssra
, ssrb
, ssrat
, ssrbt
;
1763 m3x3_skew_symetric( ssrat
, ra
);
1764 m3x3_skew_symetric( ssrbt
, rb
);
1765 m3x3_transpose( ssrat
, ssra
);
1766 m3x3_transpose( ssrbt
, ssrb
);
1768 m3x3_mul( ssra
, rba
->iIw
, cma
);
1769 m3x3_mul( cma
, ssrat
, cma
);
1770 m3x3_mul( ssrb
, rbb
->iIw
, cmb
);
1771 m3x3_mul( cmb
, ssrbt
, cmb
);
1774 m3x3_add( cma
, cmb
, A
);
1775 m3x3_inv( A
, invA
);
1778 m3x3_mulv( ssra
, rba
->w
, b_wa
);
1779 m3x3_mulv( ssrb
, rbb
->w
, b_wb
);
1780 v3_add( b_wa
, b_wb
, b
);
1784 m3x3_mulv( invA
, b
, impulse
);
1786 v3f delta_wa
, delta_wb
;
1788 m3x3_mul( rba
->iIw
, ssrat
, iwa
);
1789 m3x3_mul( rbb
->iIw
, ssrbt
, iwb
);
1790 m3x3_mulv( iwa
, impulse
, delta_wa
);
1791 m3x3_mulv( iwb
, impulse
, delta_wb
);
1792 v3_add( rba
->w
, delta_wa
, rba
->w
);
1793 v3_sub( rbb
->w
, delta_wb
, rbb
->w
);
1797 * Correct position constraint drift errors
1798 * [ 0.0 <= amt <= 1.0 ]: the correction amount
1800 static void rb_correct_position_constraints( rb_constr_pos
*buf
, int len
,
1802 for( int i
=0; i
<len
; i
++ ){
1803 rb_constr_pos
*constr
= &buf
[i
];
1804 rigidbody
*rba
= constr
->rba
, *rbb
= constr
->rbb
;
1807 m3x3_mulv( rba
->to_world
, constr
->lca
, p0
);
1808 m3x3_mulv( rbb
->to_world
, constr
->lcb
, p1
);
1809 v3_add( rba
->co
, p0
, p0
);
1810 v3_add( rbb
->co
, p1
, p1
);
1811 v3_sub( p1
, p0
, d
);
1814 v3_muladds( rbb
->co
, d
, -1.0f
* amt
, rbb
->co
);
1815 rb_update_matrices( rbb
);
1817 f32 mt
= 1.0f
/(rba
->inv_mass
+rbb
->inv_mass
),
1818 a
= mt
* (k_phys_baumgarte
/k_rb_delta
);
1820 v3_muladds( rba
->v
, d
, a
* rba
->inv_mass
, rba
->v
);
1821 v3_muladds( rbb
->v
, d
, a
*-rbb
->inv_mass
, rbb
->v
);
1826 static void rb_correct_swingtwist_constraints( rb_constr_swingtwist
*buf
,
1827 int len
, float amt
){
1828 for( int i
=0; i
<len
; i
++ ){
1829 rb_constr_swingtwist
*st
= &buf
[i
];
1831 if( !st
->tangent_violation
)
1835 m3x3_mulv( st
->rbb
->to_world
, st
->coneva
, va
);
1837 f32 angle
= v3_dot( va
, st
->tangent_target
);
1839 if( fabsf(angle
) < 0.9999f
){
1841 v3_cross( va
, st
->tangent_target
, axis
);
1843 angle
= acosf(angle
) * amt
;
1845 q_axis_angle( correction
, axis
, angle
);
1846 q_mul( correction
, st
->rbb
->q
, st
->rbb
->q
);
1847 q_normalize( st
->rbb
->q
);
1848 rb_update_matrices( st
->rbb
);
1850 f32 mt
= 1.0f
/(st
->rba
->inv_mass
+st
->rbb
->inv_mass
),
1851 wa
= mt
* acosf(angle
) * (k_phys_baumgarte
/k_rb_delta
);
1852 //v3_muladds( st->rba->w, axis, wa*-st->rba->inv_mass, st->rba->w );
1853 v3_muladds( st
->rbb
->w
, axis
, wa
* st
->rbb
->inv_mass
, st
->rbb
->w
);
1858 for( int i
=0; i
<len
; i
++ ){
1859 rb_constr_swingtwist
*st
= &buf
[i
];
1861 if( !st
->axis_violation
)
1865 m3x3_mulv( st
->rbb
->to_world
, st
->conevxb
, vxb
);
1867 f32 angle
= v3_dot( vxb
, st
->axis_target
);
1869 if( fabsf(angle
) < 0.9999f
){
1871 v3_cross( vxb
, st
->axis_target
, axis
);
1874 angle
= acosf(angle
) * amt
;
1876 q_axis_angle( correction
, axis
, angle
);
1877 q_mul( correction
, st
->rbb
->q
, st
->rbb
->q
);
1878 q_normalize( st
->rbb
->q
);
1879 rb_update_matrices( st
->rbb
);
1881 f32 mt
= 1.0f
/(st
->rba
->inv_mass
+st
->rbb
->inv_mass
),
1882 wa
= mt
* acosf(angle
) * (k_phys_baumgarte
/k_rb_delta
);
1883 //v3_muladds( st->rba->w, axis, wa*-0.5f, st->rba->w );
1884 v3_muladds( st
->rbb
->w
, axis
, wa
* st
->rbb
->inv_mass
, st
->rbb
->w
);
1890 static void rb_correct_contact_constraints( rb_ct
*buf
, int len
, float amt
){
1891 for( int i
=0; i
<len
; i
++ ){
1892 rb_ct
*ct
= &buf
[i
];
1893 rigidbody
*rba
= ct
->rba
,
1896 f32 mass_total
= 1.0f
/ (rba
->inv_mass
+ rbb
->inv_mass
),
1897 d
= ct
->p
*mass_total
*amt
;
1899 v3_muladds( rba
->co
, ct
->n
, -d
* rba
->inv_mass
, rba
->co
);
1900 v3_muladds( rbb
->co
, ct
->n
, d
* rbb
->inv_mass
, rbb
->co
);
1909 static void rb_effect_simple_bouyency( rigidbody
*ra
, v4f plane
,
1910 float amt
, float drag
){
1912 float depth
= v3_dot( plane
, ra
->co
) - plane
[3],
1913 lambda
= vg_clampf( -depth
, 0.0f
, 1.0f
) * amt
;
1915 v3_muladds( ra
->v
, plane
, lambda
* k_rb_delta
, ra
->v
);
1918 v3_muls( ra
->v
, 1.0f
-(drag
*k_rb_delta
), ra
->v
);
1921 /* apply a spring&dampener force to match ra(worldspace) on rigidbody, to
1924 static void rb_effect_spring_target_vector( rigidbody
*rba
, v3f ra
, v3f rt
,
1925 float spring
, float dampening
,
1927 float d
= v3_dot( rt
, ra
);
1928 float a
= acosf( vg_clampf( d
, -1.0f
, 1.0f
) );
1931 v3_cross( rt
, ra
, axis
);
1933 float Fs
= -a
* spring
,
1934 Fd
= -v3_dot( rba
->w
, axis
) * dampening
;
1936 v3_muladds( rba
->w
, axis
, (Fs
+Fd
) * timestep
, rba
->w
);
1939 #endif /* RIGIDBODY_H */