1 #include "vg_rigidbody.h"
2 #include "vg_rigidbody_collision.h"
5 #include "vg_platform.h"
7 int rb_contact_count
= 0;
8 struct rb_ct rb_contact_buffer
[VG_MAX_CONTACTS
];
13 * These do not automatically allocate contacts, an appropriately sized
14 * buffer must be supplied. The function returns the size of the manifold
15 * which was generated.
17 * The values set on the contacts are: n, co, p, rba, rbb
21 * By collecting the minimum(time) and maximum(time) pairs of points, we
22 * build a reduced and stable exact manifold.
25 * rx: minimum distance of these points
26 * dx: the delta between the two points
28 * pairs will only ammend these if they are creating a collision
30 typedef struct capsule_manifold capsule_manifold
;
31 struct capsule_manifold
{
38 * Expand a line manifold with a new pair. t value is the time along segment
39 * on the oriented object which created this pair.
41 static void rb_capsule_manifold( v3f pa
, v3f pb
, f32 t
, f32 r
,
42 capsule_manifold
*manifold
){
44 v3_sub( pa
, pb
, delta
);
46 if( v3_length2(delta
) < r
*r
){
47 if( t
< manifold
->t0
){
48 v3_copy( delta
, manifold
->d0
);
53 if( t
> manifold
->t1
){
54 v3_copy( delta
, manifold
->d1
);
61 static void rb_capsule_manifold_init( capsule_manifold
*manifold
){
62 manifold
->t0
= INFINITY
;
63 manifold
->t1
= -INFINITY
;
66 static int rb_capsule__manifold_done( m4x3f mtx
, rb_capsule
*c
,
67 capsule_manifold
*manifold
,
70 v3_muladds( mtx
[3], mtx
[1], -c
->h
*0.5f
+c
->r
, p0
);
71 v3_muladds( mtx
[3], mtx
[1], c
->h
*0.5f
-c
->r
, p1
);
74 if( manifold
->t0
<= 1.0f
){
78 v3_muls( p0
, 1.0f
-manifold
->t0
, pa
);
79 v3_muladds( pa
, p1
, manifold
->t0
, pa
);
81 f32 d
= v3_length( manifold
->d0
);
82 v3_muls( manifold
->d0
, 1.0f
/d
, ct
->n
);
83 v3_muladds( pa
, ct
->n
, -c
->r
, ct
->co
);
85 ct
->p
= manifold
->r0
- d
;
86 ct
->type
= k_contact_type_default
;
90 if( (manifold
->t1
>= 0.0f
) && (manifold
->t0
!= manifold
->t1
) ){
91 rb_ct
*ct
= buf
+count
;
94 v3_muls( p0
, 1.0f
-manifold
->t1
, pa
);
95 v3_muladds( pa
, p1
, manifold
->t1
, pa
);
97 f32 d
= v3_length( manifold
->d1
);
98 v3_muls( manifold
->d1
, 1.0f
/d
, ct
->n
);
99 v3_muladds( pa
, ct
->n
, -c
->r
, ct
->co
);
101 ct
->p
= manifold
->r1
- d
;
102 ct
->type
= k_contact_type_default
;
113 vg_line( buf
[0].co
, buf
[1].co
, 0xff0000ff );
119 int rb_capsule__sphere( m4x3f mtxA
, rb_capsule
*ca
,
120 v3f coB
, f32 rb
, rb_ct
*buf
){
126 v3_muladds( mtxA
[3], mtxA
[1], -ha
*0.5f
+ra
, p0
);
127 v3_muladds( mtxA
[3], mtxA
[1], ha
*0.5f
-ra
, p1
);
130 closest_point_segment( p0
, p1
, coB
, c
);
131 v3_sub( c
, coB
, delta
);
132 f32 d2
= v3_length2(delta
);
138 v3_muls( delta
, 1.0f
/d
, ct
->n
);
142 v3_muladds( c
, ct
->n
, -ra
, p0
);
143 v3_muladds( coB
, ct
->n
, rb
, p1
);
144 v3_add( p0
, p1
, ct
->co
);
145 v3_muls( ct
->co
, 0.5f
, ct
->co
);
146 ct
->type
= k_contact_type_default
;
152 int rb_capsule__capsule( m4x3f mtxA
, rb_capsule
*ca
,
153 m4x3f mtxB
, rb_capsule
*cb
, rb_ct
*buf
)
162 v3_muladds( mtxA
[3], mtxA
[1], -ha
*0.5f
+ra
, p0
);
163 v3_muladds( mtxA
[3], mtxA
[1], ha
*0.5f
-ra
, p1
);
164 v3_muladds( mtxB
[3], mtxB
[1], -hb
*0.5f
+rb
, p2
);
165 v3_muladds( mtxB
[3], mtxB
[1], hb
*0.5f
-rb
, p3
);
167 capsule_manifold manifold
;
168 rb_capsule_manifold_init( &manifold
);
172 closest_segment_segment( p0
, p1
, p2
, p3
, &ta
, &tb
, pa
, pb
);
173 rb_capsule_manifold( pa
, pb
, ta
, r
, &manifold
);
175 ta
= closest_point_segment( p0
, p1
, p2
, pa
);
176 tb
= closest_point_segment( p0
, p1
, p3
, pb
);
177 rb_capsule_manifold( pa
, p2
, ta
, r
, &manifold
);
178 rb_capsule_manifold( pb
, p3
, tb
, r
, &manifold
);
180 closest_point_segment( p2
, p3
, p0
, pa
);
181 closest_point_segment( p2
, p3
, p1
, pb
);
182 rb_capsule_manifold( p0
, pa
, 0.0f
, r
, &manifold
);
183 rb_capsule_manifold( p1
, pb
, 1.0f
, r
, &manifold
);
185 return rb_capsule__manifold_done( mtxA
, ca
, &manifold
, buf
);
189 * Generates up to two contacts; optimised for the most stable manifold
191 int rb_capsule__box( m4x3f mtxA
, rb_capsule
*ca
,
192 m4x3f mtxB
, m4x3f mtxB_inverse
, boxf box
,
195 f32 h
= ca
->h
, r
= ca
->r
;
198 * Solving this in symetric local space of the cube saves us some time and a
199 * couple branches when it comes to the quad stage.
202 v3_add( box
[0], box
[1], centroid
);
203 v3_muls( centroid
, 0.5f
, centroid
);
206 v3_sub( box
[0], centroid
, bbx
[0] );
207 v3_sub( box
[1], centroid
, bbx
[1] );
209 v3f pc
, p0w
, p1w
, p0
, p1
;
210 v3_muladds( mtxA
[3], mtxA
[1], -h
*0.5f
+r
, p0w
);
211 v3_muladds( mtxA
[3], mtxA
[1], h
*0.5f
-r
, p1w
);
213 m4x3_mulv( mtxB_inverse
, p0w
, p0
);
214 m4x3_mulv( mtxB_inverse
, p1w
, p1
);
215 v3_sub( p0
, centroid
, p0
);
216 v3_sub( p1
, centroid
, p1
);
217 v3_add( p0
, p1
, pc
);
218 v3_muls( pc
, 0.5f
, pc
);
221 * Finding an appropriate quad to collide lines with
224 v3_div( pc
, bbx
[1], region
);
227 if( (fabsf(region
[0]) > fabsf(region
[1])) &&
228 (fabsf(region
[0]) > fabsf(region
[2])) )
230 f32 px
= vg_signf(region
[0]) * bbx
[1][0];
231 v3_copy( (v3f
){ px
, bbx
[0][1], bbx
[0][2] }, quad
[0] );
232 v3_copy( (v3f
){ px
, bbx
[1][1], bbx
[0][2] }, quad
[1] );
233 v3_copy( (v3f
){ px
, bbx
[1][1], bbx
[1][2] }, quad
[2] );
234 v3_copy( (v3f
){ px
, bbx
[0][1], bbx
[1][2] }, quad
[3] );
236 else if( fabsf(region
[1]) > fabsf(region
[2]) )
238 f32 py
= vg_signf(region
[1]) * bbx
[1][1];
239 v3_copy( (v3f
){ bbx
[0][0], py
, bbx
[0][2] }, quad
[0] );
240 v3_copy( (v3f
){ bbx
[1][0], py
, bbx
[0][2] }, quad
[1] );
241 v3_copy( (v3f
){ bbx
[1][0], py
, bbx
[1][2] }, quad
[2] );
242 v3_copy( (v3f
){ bbx
[0][0], py
, bbx
[1][2] }, quad
[3] );
246 f32 pz
= vg_signf(region
[2]) * bbx
[1][2];
247 v3_copy( (v3f
){ bbx
[0][0], bbx
[0][1], pz
}, quad
[0] );
248 v3_copy( (v3f
){ bbx
[1][0], bbx
[0][1], pz
}, quad
[1] );
249 v3_copy( (v3f
){ bbx
[1][0], bbx
[1][1], pz
}, quad
[2] );
250 v3_copy( (v3f
){ bbx
[0][0], bbx
[1][1], pz
}, quad
[3] );
253 capsule_manifold manifold
;
254 rb_capsule_manifold_init( &manifold
);
257 closest_point_aabb( p0
, bbx
, c0
);
258 closest_point_aabb( p1
, bbx
, c1
);
261 v3_sub( c0
, p0
, d0
);
262 v3_sub( c1
, p1
, d1
);
263 v3_sub( p1
, p0
, da
);
270 if( v3_dot( da
, d0
) <= 0.01f
)
271 rb_capsule_manifold( p0
, c0
, 0.0f
, r
, &manifold
);
273 if( v3_dot( da
, d1
) >= -0.01f
)
274 rb_capsule_manifold( p1
, c1
, 1.0f
, r
, &manifold
);
276 for( i32 i
=0; i
<4; i
++ ){
282 closest_segment_segment( p0
, p1
, quad
[i0
], quad
[i1
], &ta
, &tb
, ca
, cb
);
283 rb_capsule_manifold( ca
, cb
, ta
, r
, &manifold
);
287 * Create final contacts based on line manifold
289 m3x3_mulv( mtxB
, manifold
.d0
, manifold
.d0
);
290 m3x3_mulv( mtxB
, manifold
.d1
, manifold
.d1
);
291 return rb_capsule__manifold_done( mtxA
, ca
, &manifold
, buf
);
294 int rb_sphere__box( v3f coA
, f32 ra
,
295 m4x3f mtxB
, m4x3f mtxB_inverse
, boxf box
,
299 closest_point_obb( coA
, box
, mtxB
, mtxB_inverse
, co
);
300 v3_sub( coA
, co
, delta
);
302 f32 d2
= v3_length2(delta
);
310 v3_sub( box
[1], box
[0], e
);
311 v3_muls( e
, 0.5f
, e
);
312 v3_add( box
[0], e
, coB
);
313 v3_sub( coA
, coB
, delta
);
316 * some extra testing is required to find the best axis to push the
317 * object back outside the box. Since there isnt a clear seperating
318 * vector already, especially on really high aspect boxes.
320 f32 lx
= v3_dot( mtxB
[0], delta
),
321 ly
= v3_dot( mtxB
[1], delta
),
322 lz
= v3_dot( mtxB
[2], delta
),
323 px
= e
[0] - fabsf(lx
),
324 py
= e
[1] - fabsf(ly
),
325 pz
= e
[2] - fabsf(lz
);
327 if( px
< py
&& px
< pz
) v3_muls( mtxB
[0], vg_signf(lx
), ct
->n
);
328 else if( py
< pz
) v3_muls( mtxB
[1], vg_signf(ly
), ct
->n
);
329 else v3_muls( mtxB
[2], vg_signf(lz
), ct
->n
);
331 v3_muladds( coA
, ct
->n
, -ra
, ct
->co
);
336 v3_muls( delta
, 1.0f
/d
, ct
->n
);
338 v3_copy( co
, ct
->co
);
341 ct
->type
= k_contact_type_default
;
347 int rb_sphere__sphere( v3f coA
, f32 ra
, v3f coB
, f32 rb
, rb_ct
*buf
)
350 v3_sub( coA
, coB
, delta
);
352 f32 d2
= v3_length2(delta
),
359 v3_muls( delta
, 1.0f
/d
, ct
->n
);
362 v3_muladds( coA
, ct
->n
,-ra
, p0
);
363 v3_muladds( coB
, ct
->n
, rb
, p1
);
364 v3_add( p0
, p1
, ct
->co
);
365 v3_muls( ct
->co
, 0.5f
, ct
->co
);
366 ct
->type
= k_contact_type_default
;
373 int rb_sphere__triangle( m4x3f mtxA
, f32 r
, v3f tri
[3], rb_ct
*buf
)
376 enum contact_type type
= closest_on_triangle_1( mtxA
[3], tri
, co
);
377 v3_sub( mtxA
[3], co
, delta
);
378 f32 d2
= v3_length2( delta
);
384 v3_sub( tri
[2], tri
[0], ab
);
385 v3_sub( tri
[1], tri
[0], ac
);
386 v3_cross( ac
, ab
, tn
);
387 v3_copy( tn
, ct
->n
);
389 if( v3_length2( ct
->n
) <= 0.00001f
){
390 #ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING
391 vg_error( "Zero area triangle!\n" );
396 v3_normalize( ct
->n
);
400 v3_copy( co
, ct
->co
);
409 int rb_capsule__triangle( m4x3f mtxA
, rb_capsule
*c
, v3f tri
[3], rb_ct
*buf
)
412 v3_muladds( mtxA
[3], mtxA
[1], -c
->h
*0.5f
+c
->r
, p0w
);
413 v3_muladds( mtxA
[3], mtxA
[1], c
->h
*0.5f
-c
->r
, p1w
);
415 capsule_manifold manifold
;
416 rb_capsule_manifold_init( &manifold
);
419 v3_sub( tri
[1], tri
[0], v0
);
420 v3_sub( tri
[2], tri
[0], v1
);
421 v3_cross( v0
, v1
, n
);
423 if( v3_length2( n
) <= 0.00001f
){
424 #ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING
425 vg_error( "Zero area triangle!\n" );
433 /* deep penetration recovery. for when we clip through the triangles. so its
434 * not very 'correct' */
436 if( ray_tri( tri
, p0w
, mtxA
[1], &dist
, 1 ) ){
437 f32 l
= c
->h
- c
->r
*2.0f
;
438 if( (dist
>= 0.0f
) && (dist
< l
) ){
440 v3_muladds( p0w
, mtxA
[1], dist
, co
);
441 vg_line_point( co
, 0.02f
, 0xffffff00 );
444 v3_sub( p0w
, co
, d0
);
445 v3_sub( p1w
, co
, d1
);
447 f32 p
= vg_minf( v3_dot( n
, d0
), v3_dot( n
, d1
) ) - c
->r
;
451 ct
->type
= k_contact_type_default
;
453 v3_muladds( co
, n
, p
, ct
->co
);
461 closest_on_triangle_1( p0w
, tri
, c0
);
462 closest_on_triangle_1( p1w
, tri
, c1
);
465 v3_sub( c0
, p0w
, d0
);
466 v3_sub( c1
, p1w
, d1
);
467 v3_sub( p1w
, p0w
, da
);
473 /* the two balls at the ends */
474 if( v3_dot( da
, d0
) <= 0.01f
)
475 rb_capsule_manifold( p0w
, c0
, 0.0f
, c
->r
, &manifold
);
476 if( v3_dot( da
, d1
) >= -0.01f
)
477 rb_capsule_manifold( p1w
, c1
, 1.0f
, c
->r
, &manifold
);
479 /* the edges to edges */
480 for( int i
=0; i
<3; i
++ ){
486 closest_segment_segment( p0w
, p1w
, tri
[i0
], tri
[i1
], &ta
, &tb
, ca
, cb
);
487 rb_capsule_manifold( ca
, cb
, ta
, c
->r
, &manifold
);
490 int count
= rb_capsule__manifold_done( mtxA
, c
, &manifold
, buf
);
491 for( int i
=0; i
<count
; i
++ )
492 v3_copy( n
, buf
[i
].n
);
497 int rb_global_has_space( void )
499 if( rb_contact_count
+ 16 > vg_list_size(rb_contact_buffer
) )
505 rb_ct
*rb_global_buffer( void )
507 return &rb_contact_buffer
[ rb_contact_count
];
511 * -----------------------------------------------------------------------------
512 * Boolean shape overlap functions
513 * -----------------------------------------------------------------------------
517 * Project AABB, and triangle interval onto axis to check if they overlap
519 static int rb_box_triangle_interval( v3f extent
, v3f axis
, v3f tri
[3] ){
522 r
= extent
[0] * fabsf(axis
[0]) +
523 extent
[1] * fabsf(axis
[1]) +
524 extent
[2] * fabsf(axis
[2]),
526 p0
= v3_dot( axis
, tri
[0] ),
527 p1
= v3_dot( axis
, tri
[1] ),
528 p2
= v3_dot( axis
, tri
[2] ),
530 e
= vg_maxf(-vg_maxf(p0
,vg_maxf(p1
,p2
)), vg_minf(p0
,vg_minf(p1
,p2
)));
532 if( e
> r
) return 0;
537 * Seperating axis test box vs triangle
539 int rb_box_triangle_sat( v3f extent
, v3f center
,
540 m4x3f to_local
, v3f tri_src
[3] )
544 for( int i
=0; i
<3; i
++ ){
545 m4x3_mulv( to_local
, tri_src
[i
], tri
[i
] );
546 v3_sub( tri
[i
], center
, tri
[i
] );
550 v3_sub( tri
[1], tri
[0], f0
);
551 v3_sub( tri
[2], tri
[1], f1
);
552 v3_sub( tri
[0], tri
[2], f2
);
556 v3_cross( (v3f
){1.0f
,0.0f
,0.0f
}, f0
, axis
[0] );
557 v3_cross( (v3f
){1.0f
,0.0f
,0.0f
}, f1
, axis
[1] );
558 v3_cross( (v3f
){1.0f
,0.0f
,0.0f
}, f2
, axis
[2] );
559 v3_cross( (v3f
){0.0f
,1.0f
,0.0f
}, f0
, axis
[3] );
560 v3_cross( (v3f
){0.0f
,1.0f
,0.0f
}, f1
, axis
[4] );
561 v3_cross( (v3f
){0.0f
,1.0f
,0.0f
}, f2
, axis
[5] );
562 v3_cross( (v3f
){0.0f
,0.0f
,1.0f
}, f0
, axis
[6] );
563 v3_cross( (v3f
){0.0f
,0.0f
,1.0f
}, f1
, axis
[7] );
564 v3_cross( (v3f
){0.0f
,0.0f
,1.0f
}, f2
, axis
[8] );
566 for( int i
=0; i
<9; i
++ )
567 if(!rb_box_triangle_interval( extent
, axis
[i
], tri
)) return 0;
570 if(!rb_box_triangle_interval( extent
, (v3f
){1.0f
,0.0f
,0.0f
}, tri
)) return 0;
571 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,1.0f
,0.0f
}, tri
)) return 0;
572 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,0.0f
,1.0f
}, tri
)) return 0;
575 v3_cross( f0
, f1
, n
);
576 if(!rb_box_triangle_interval( extent
, n
, tri
)) return 0;
582 * -----------------------------------------------------------------------------
584 * -----------------------------------------------------------------------------
587 int rb_manifold_apply_filtered( rb_ct
*man
, int len
)
591 for( int i
=0; i
<len
; i
++ ){
594 if( ct
->type
== k_contact_type_disabled
)
603 void rb_manifold_contact_weld( rb_ct
*ci
, rb_ct
*cj
, float r
)
605 if( v3_dist2( ci
->co
, cj
->co
) < r
*r
){
606 cj
->type
= k_contact_type_disabled
;
607 ci
->p
= (ci
->p
+ cj
->p
) * 0.5f
;
609 v3_add( ci
->co
, cj
->co
, ci
->co
);
610 v3_muls( ci
->co
, 0.5f
, ci
->co
);
613 v3_sub( ci
->rba
->co
, ci
->co
, delta
);
615 float c0
= v3_dot( ci
->n
, delta
),
616 c1
= v3_dot( cj
->n
, delta
);
618 if( c0
< 0.0f
|| c1
< 0.0f
){
620 ci
->type
= k_contact_type_disabled
;
624 v3_muls( ci
->n
, c0
, n
);
625 v3_muladds( n
, cj
->n
, c1
, n
);
635 void rb_manifold_filter_joint_edges( rb_ct
*man
, int len
, float r
)
637 for( int i
=0; i
<len
-1; i
++ ){
639 if( ci
->type
!= k_contact_type_edge
)
642 for( int j
=i
+1; j
<len
; j
++ ){
644 if( cj
->type
!= k_contact_type_edge
)
647 rb_manifold_contact_weld( ci
, cj
, r
);
652 void rb_manifold_filter_pairs( rb_ct
*man
, int len
, float r
)
654 for( int i
=0; i
<len
-1; i
++ ){
658 if( ci
->type
== k_contact_type_disabled
) continue;
660 for( int j
=i
+1; j
<len
; j
++ ){
663 if( cj
->type
== k_contact_type_disabled
) continue;
665 if( v3_dist2( ci
->co
, cj
->co
) < r
*r
){
666 cj
->type
= k_contact_type_disabled
;
667 v3_add( cj
->n
, ci
->n
, ci
->n
);
674 float n
= 1.0f
/((float)similar
+1.0f
);
675 v3_muls( ci
->n
, n
, ci
->n
);
678 if( v3_length2(ci
->n
) < 0.1f
*0.1f
)
679 ci
->type
= k_contact_type_disabled
;
681 v3_normalize( ci
->n
);
686 void rb_manifold_filter_backface( rb_ct
*man
, int len
)
688 for( int i
=0; i
<len
; i
++ ){
690 if( ct
->type
== k_contact_type_disabled
)
694 v3_sub( ct
->co
, ct
->rba
->co
, delta
);
696 if( v3_dot( delta
, ct
->n
) > -0.001f
)
697 ct
->type
= k_contact_type_disabled
;
701 void rb_manifold_filter_coplanar( rb_ct
*man
, int len
, float w
)
703 for( int i
=0; i
<len
; i
++ ){
705 if( ci
->type
== k_contact_type_disabled
||
706 ci
->type
== k_contact_type_edge
)
709 float d1
= v3_dot( ci
->co
, ci
->n
);
711 for( int j
=0; j
<len
; j
++ ){
716 if( cj
->type
== k_contact_type_disabled
)
719 float d2
= v3_dot( cj
->co
, ci
->n
),
722 if( fabsf( d
) <= w
){
723 cj
->type
= k_contact_type_disabled
;
729 void rb_debug_contact( rb_ct
*ct
)
732 v3_muladds( ct
->co
, ct
->n
, 0.05f
, p1
);
734 if( ct
->type
== k_contact_type_default
){
735 vg_line_point( ct
->co
, 0.0125f
, 0xff0000ff );
736 vg_line( ct
->co
, p1
, 0xffffffff );
738 else if( ct
->type
== k_contact_type_edge
){
739 vg_line_point( ct
->co
, 0.0125f
, 0xff00ffc0 );
740 vg_line( ct
->co
, p1
, 0xffffffff );
744 void rb_solver_reset(void)
746 rb_contact_count
= 0;
749 rb_ct
*rb_global_ct(void)
751 return rb_contact_buffer
+ rb_contact_count
;
754 void rb_prepare_contact( rb_ct
*ct
, f32 dt
)
756 ct
->bias
= -k_phys_baumgarte
* (dt
*3600.0f
)
757 * vg_minf( 0.0f
, -ct
->p
+k_penetration_slop
);
759 v3_tangent_basis( ct
->n
, ct
->t
[0], ct
->t
[1] );
760 ct
->norm_impulse
= 0.0f
;
761 ct
->tangent_impulse
[0] = 0.0f
;
762 ct
->tangent_impulse
[1] = 0.0f
;
766 * calculate total move to depenetrate object from contacts.
767 * manifold should belong to ONE object only
769 void rb_depenetrate( rb_ct
*manifold
, int len
, v3f dt
)
773 for( int j
=0; j
<7; j
++ ){
774 for( int i
=0; i
<len
; i
++ ){
775 rb_ct
*ct
= &manifold
[i
];
777 float resolved_amt
= v3_dot( ct
->n
, dt
),
778 remaining
= (ct
->p
-k_penetration_slop
) - resolved_amt
,
779 apply
= vg_maxf( remaining
, 0.0f
) * 0.4f
;
781 v3_muladds( dt
, ct
->n
, apply
, dt
);
787 * Initializing things like tangent vectors
789 void rb_presolve_contacts( rb_ct
*buffer
, f32 dt
, int len
)
791 for( int i
=0; i
<len
; i
++ ){
792 rb_ct
*ct
= &buffer
[i
];
793 rb_prepare_contact( ct
, dt
);
795 v3f ra
, rb
, raCn
, rbCn
, raCt
, rbCt
;
796 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
797 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
798 v3_cross( ra
, ct
->n
, raCn
);
799 v3_cross( rb
, ct
->n
, rbCn
);
801 /* orient inverse inertia tensors */
803 m3x3_mulv( ct
->rba
->iIw
, raCn
, raCnI
);
804 m3x3_mulv( ct
->rbb
->iIw
, rbCn
, rbCnI
);
806 ct
->normal_mass
= ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
807 ct
->normal_mass
+= v3_dot( raCn
, raCnI
);
808 ct
->normal_mass
+= v3_dot( rbCn
, rbCnI
);
809 ct
->normal_mass
= 1.0f
/ct
->normal_mass
;
811 for( int j
=0; j
<2; j
++ ){
813 v3_cross( ct
->t
[j
], ra
, raCt
);
814 v3_cross( ct
->t
[j
], rb
, rbCt
);
815 m3x3_mulv( ct
->rba
->iIw
, raCt
, raCtI
);
816 m3x3_mulv( ct
->rbb
->iIw
, rbCt
, rbCtI
);
818 ct
->tangent_mass
[j
] = ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
819 ct
->tangent_mass
[j
] += v3_dot( raCt
, raCtI
);
820 ct
->tangent_mass
[j
] += v3_dot( rbCt
, rbCtI
);
821 ct
->tangent_mass
[j
] = 1.0f
/ct
->tangent_mass
[j
];
826 void rb_contact_restitution( rb_ct
*ct
, float cr
)
829 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
830 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
831 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
833 float v
= v3_dot( rv
, ct
->n
);
841 * One iteration to solve the contact constraint
843 void rb_solve_contacts( rb_ct
*buf
, int len
)
845 for( int i
=0; i
<len
; i
++ ){
849 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
850 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
851 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
854 for( int j
=0; j
<2; j
++ ){
855 float f
= k_friction
* ct
->norm_impulse
,
856 vt
= v3_dot( rv
, ct
->t
[j
] ),
857 lambda
= ct
->tangent_mass
[j
] * -vt
;
859 float temp
= ct
->tangent_impulse
[j
];
860 ct
->tangent_impulse
[j
] = vg_clampf( temp
+ lambda
, -f
, f
);
861 lambda
= ct
->tangent_impulse
[j
] - temp
;
864 v3_muls( ct
->t
[j
], lambda
, impulse
);
865 rb_linear_impulse( ct
->rba
, ra
, impulse
);
867 v3_muls( ct
->t
[j
], -lambda
, impulse
);
868 rb_linear_impulse( ct
->rbb
, rb
, impulse
);
872 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
873 float vn
= v3_dot( rv
, ct
->n
),
874 lambda
= ct
->normal_mass
* (-vn
+ ct
->bias
);
876 float temp
= ct
->norm_impulse
;
877 ct
->norm_impulse
= vg_maxf( temp
+ lambda
, 0.0f
);
878 lambda
= ct
->norm_impulse
- temp
;
881 v3_muls( ct
->n
, lambda
, impulse
);
882 rb_linear_impulse( ct
->rba
, ra
, impulse
);
884 v3_muls( ct
->n
, -lambda
, impulse
);
885 rb_linear_impulse( ct
->rbb
, rb
, impulse
);