2 #include "vg_rigidbody.h"
4 typedef struct rb_ct rb_ct
;
9 float p
, bias
, norm_impulse
, tangent_impulse
[2],
10 normal_mass
, tangent_mass
[2];
14 enum contact_type type
;
16 rb_contact_buffer
[256];
17 static int rb_contact_count
= 0;
22 * These do not automatically allocate contacts, an appropriately sized
23 * buffer must be supplied. The function returns the size of the manifold
24 * which was generated.
26 * The values set on the contacts are: n, co, p, rba, rbb
30 * By collecting the minimum(time) and maximum(time) pairs of points, we
31 * build a reduced and stable exact manifold.
34 * rx: minimum distance of these points
35 * dx: the delta between the two points
37 * pairs will only ammend these if they are creating a collision
39 typedef struct capsule_manifold capsule_manifold
;
40 struct capsule_manifold
{
47 * Expand a line manifold with a new pair. t value is the time along segment
48 * on the oriented object which created this pair.
50 static void rb_capsule_manifold( v3f pa
, v3f pb
, f32 t
, f32 r
,
51 capsule_manifold
*manifold
){
53 v3_sub( pa
, pb
, delta
);
55 if( v3_length2(delta
) < r
*r
){
56 if( t
< manifold
->t0
){
57 v3_copy( delta
, manifold
->d0
);
62 if( t
> manifold
->t1
){
63 v3_copy( delta
, manifold
->d1
);
70 static void rb_capsule_manifold_init( capsule_manifold
*manifold
){
71 manifold
->t0
= INFINITY
;
72 manifold
->t1
= -INFINITY
;
75 static int rb_capsule__manifold_done( m4x3f mtx
, rb_capsule
*c
,
76 capsule_manifold
*manifold
,
79 v3_muladds( mtx
[3], mtx
[1], -c
->h
*0.5f
+c
->r
, p0
);
80 v3_muladds( mtx
[3], mtx
[1], c
->h
*0.5f
-c
->r
, p1
);
83 if( manifold
->t0
<= 1.0f
){
87 v3_muls( p0
, 1.0f
-manifold
->t0
, pa
);
88 v3_muladds( pa
, p1
, manifold
->t0
, pa
);
90 f32 d
= v3_length( manifold
->d0
);
91 v3_muls( manifold
->d0
, 1.0f
/d
, ct
->n
);
92 v3_muladds( pa
, ct
->n
, -c
->r
, ct
->co
);
94 ct
->p
= manifold
->r0
- d
;
95 ct
->type
= k_contact_type_default
;
99 if( (manifold
->t1
>= 0.0f
) && (manifold
->t0
!= manifold
->t1
) ){
100 rb_ct
*ct
= buf
+count
;
103 v3_muls( p0
, 1.0f
-manifold
->t1
, pa
);
104 v3_muladds( pa
, p1
, manifold
->t1
, pa
);
106 f32 d
= v3_length( manifold
->d1
);
107 v3_muls( manifold
->d1
, 1.0f
/d
, ct
->n
);
108 v3_muladds( pa
, ct
->n
, -c
->r
, ct
->co
);
110 ct
->p
= manifold
->r1
- d
;
111 ct
->type
= k_contact_type_default
;
121 vg_line( buf
[0].co
, buf
[1].co
, 0xff0000ff );
127 static int rb_capsule_sphere( rb_object
*obja
, rb_object
*objb
, rb_ct
*buf
){
128 rigidbody
*rba
= &obja
->rb
, *rbb
= &objb
->rb
;
129 f32 h
= obja
->inf
.capsule
.h
,
130 ra
= obja
->inf
.capsule
.r
,
131 rb
= objb
->inf
.sphere
.r
;
134 v3_muladds( rba
->co
, rba
->to_world
[1], -h
*0.5f
+ra
, p0
);
135 v3_muladds( rba
->co
, rba
->to_world
[1], h
*0.5f
-ra
, p1
);
138 closest_point_segment( p0
, p1
, rbb
->co
, c
);
139 v3_sub( c
, rbb
->co
, delta
);
141 f32 d2
= v3_length2(delta
),
148 v3_muls( delta
, 1.0f
/d
, ct
->n
);
152 v3_muladds( c
, ct
->n
, -ra
, p0
);
153 v3_muladds( rbb
->co
, ct
->n
, rb
, p1
);
154 v3_add( p0
, p1
, ct
->co
);
155 v3_muls( ct
->co
, 0.5f
, ct
->co
);
159 ct
->type
= k_contact_type_default
;
168 static int rb_capsule__capsule( m4x3f mtxA
, rb_capsule
*ca
,
169 m4x3f mtxB
, rb_capsule
*cb
, rb_ct
*buf
){
177 v3_muladds( mtxA
[3], mtxA
[1], -ha
*0.5f
+ra
, p0
);
178 v3_muladds( mtxA
[3], mtxA
[1], ha
*0.5f
-ra
, p1
);
179 v3_muladds( mtxB
[3], mtxB
[1], -hb
*0.5f
+rb
, p2
);
180 v3_muladds( mtxB
[3], mtxB
[1], hb
*0.5f
-rb
, p3
);
182 capsule_manifold manifold
;
183 rb_capsule_manifold_init( &manifold
);
187 closest_segment_segment( p0
, p1
, p2
, p3
, &ta
, &tb
, pa
, pb
);
188 rb_capsule_manifold( pa
, pb
, ta
, r
, &manifold
);
190 ta
= closest_point_segment( p0
, p1
, p2
, pa
);
191 tb
= closest_point_segment( p0
, p1
, p3
, pb
);
192 rb_capsule_manifold( pa
, p2
, ta
, r
, &manifold
);
193 rb_capsule_manifold( pb
, p3
, tb
, r
, &manifold
);
195 closest_point_segment( p2
, p3
, p0
, pa
);
196 closest_point_segment( p2
, p3
, p1
, pb
);
197 rb_capsule_manifold( p0
, pa
, 0.0f
, r
, &manifold
);
198 rb_capsule_manifold( p1
, pb
, 1.0f
, r
, &manifold
);
200 return rb_capsule__manifold_done( mtxA
, ca
, &manifold
, buf
);
204 static int rb_sphere_box( rb_object
*obja
, rb_object
*objb
, rb_ct
*buf
){
206 rigidbody
*rba
= &obja
->rb
, *rbb
= &objb
->rb
;
208 closest_point_obb( rba
->co
, rbb
->bbx
, rbb
->to_world
, rbb
->to_local
, co
);
209 v3_sub( rba
->co
, co
, delta
);
211 f32 d2
= v3_length2(delta
),
212 r
= obja
->inf
.sphere
.radius
;
219 v3_sub( rba
->co
, rbb
->co
, delta
);
222 * some extra testing is required to find the best axis to push the
223 * object back outside the box. Since there isnt a clear seperating
224 * vector already, especially on really high aspect boxes.
226 f32 lx
= v3_dot( rbb
->to_world
[0], delta
),
227 ly
= v3_dot( rbb
->to_world
[1], delta
),
228 lz
= v3_dot( rbb
->to_world
[2], delta
),
229 px
= rbb
->bbx
[1][0] - fabsf(lx
),
230 py
= rbb
->bbx
[1][1] - fabsf(ly
),
231 pz
= rbb
->bbx
[1][2] - fabsf(lz
);
233 if( px
< py
&& px
< pz
)
234 v3_muls( rbb
->to_world
[0], vg_signf(lx
), ct
->n
);
236 v3_muls( rbb
->to_world
[1], vg_signf(ly
), ct
->n
);
238 v3_muls( rbb
->to_world
[2], vg_signf(lz
), ct
->n
);
240 v3_muladds( rba
->co
, ct
->n
, -r
, ct
->co
);
245 v3_muls( delta
, 1.0f
/d
, ct
->n
);
247 v3_copy( co
, ct
->co
);
252 ct
->type
= k_contact_type_default
;
261 static int rb_sphere_sphere( rb_object
*obja
, rb_object
*objb
, rb_ct
*buf
){
262 rigidbody
*rba
= &obja
->rb
, *rbb
= &objb
->rb
;
264 v3_sub( rba
->co
, rbb
->co
, delta
);
266 f32 d2
= v3_length2(delta
),
267 r
= obja
->inf
.sphere
.radius
+ objb
->inf
.sphere
.radius
;
273 v3_muls( delta
, 1.0f
/d
, ct
->n
);
276 v3_muladds( rba
->co
, ct
->n
,-obja
->inf
.sphere
.radius
, p0
);
277 v3_muladds( rbb
->co
, ct
->n
, objb
->inf
.sphere
.radius
, p1
);
278 v3_add( p0
, p1
, ct
->co
);
279 v3_muls( ct
->co
, 0.5f
, ct
->co
);
280 ct
->type
= k_contact_type_default
;
291 static int rb_sphere__triangle( m4x3f mtxA
, f32 r
,
292 v3f tri
[3], rb_ct
*buf
){
294 enum contact_type type
= closest_on_triangle_1( mtxA
[3], tri
, co
);
295 v3_sub( mtxA
[3], co
, delta
);
296 f32 d2
= v3_length2( delta
);
302 v3_sub( tri
[2], tri
[0], ab
);
303 v3_sub( tri
[1], tri
[0], ac
);
304 v3_cross( ac
, ab
, tn
);
305 v3_copy( tn
, ct
->n
);
307 if( v3_length2( ct
->n
) <= 0.00001f
){
308 #ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING
309 vg_error( "Zero area triangle!\n" );
314 v3_normalize( ct
->n
);
318 v3_copy( co
, ct
->co
);
327 static int rb_capsule__triangle( m4x3f mtxA
, rb_capsule
*c
,
328 v3f tri
[3], rb_ct
*buf
){
330 v3_muladds( mtxA
[3], mtxA
[1], -c
->h
*0.5f
+c
->r
, p0w
);
331 v3_muladds( mtxA
[3], mtxA
[1], c
->h
*0.5f
-c
->r
, p1w
);
333 capsule_manifold manifold
;
334 rb_capsule_manifold_init( &manifold
);
337 v3_sub( tri
[1], tri
[0], v0
);
338 v3_sub( tri
[2], tri
[0], v1
);
339 v3_cross( v0
, v1
, n
);
341 if( v3_length2( n
) <= 0.00001f
){
342 #ifdef RIGIDBODY_CRY_ABOUT_EVERYTHING
343 vg_error( "Zero area triangle!\n" );
351 /* deep penetration recovery. for when we clip through the triangles. so its
352 * not very 'correct' */
354 if( ray_tri( tri
, p0w
, mtxA
[1], &dist
, 1 ) ){
355 f32 l
= c
->h
- c
->r
*2.0f
;
356 if( (dist
>= 0.0f
) && (dist
< l
) ){
358 v3_muladds( p0w
, mtxA
[1], dist
, co
);
359 vg_line_point( co
, 0.02f
, 0xffffff00 );
362 v3_sub( p0w
, co
, d0
);
363 v3_sub( p1w
, co
, d1
);
365 f32 p
= vg_minf( v3_dot( n
, d0
), v3_dot( n
, d1
) ) - c
->r
;
369 ct
->type
= k_contact_type_default
;
371 v3_muladds( co
, n
, p
, ct
->co
);
379 closest_on_triangle_1( p0w
, tri
, c0
);
380 closest_on_triangle_1( p1w
, tri
, c1
);
383 v3_sub( c0
, p0w
, d0
);
384 v3_sub( c1
, p1w
, d1
);
385 v3_sub( p1w
, p0w
, da
);
391 /* the two balls at the ends */
392 if( v3_dot( da
, d0
) <= 0.01f
)
393 rb_capsule_manifold( p0w
, c0
, 0.0f
, c
->r
, &manifold
);
394 if( v3_dot( da
, d1
) >= -0.01f
)
395 rb_capsule_manifold( p1w
, c1
, 1.0f
, c
->r
, &manifold
);
397 /* the edges to edges */
398 for( int i
=0; i
<3; i
++ ){
404 closest_segment_segment( p0w
, p1w
, tri
[i0
], tri
[i1
], &ta
, &tb
, ca
, cb
);
405 rb_capsule_manifold( ca
, cb
, ta
, c
->r
, &manifold
);
408 int count
= rb_capsule__manifold_done( mtxA
, c
, &manifold
, buf
);
409 for( int i
=0; i
<count
; i
++ )
410 v3_copy( n
, buf
[i
].n
);
415 static int rb_global_has_space( void ){
416 if( rb_contact_count
+ 16 > vg_list_size(rb_contact_buffer
) )
422 static rb_ct
*rb_global_buffer( void ){
423 return &rb_contact_buffer
[ rb_contact_count
];
427 * -----------------------------------------------------------------------------
428 * Boolean shape overlap functions
429 * -----------------------------------------------------------------------------
433 * Project AABB, and triangle interval onto axis to check if they overlap
435 static int rb_box_triangle_interval( v3f extent
, v3f axis
, v3f tri
[3] ){
438 r
= extent
[0] * fabsf(axis
[0]) +
439 extent
[1] * fabsf(axis
[1]) +
440 extent
[2] * fabsf(axis
[2]),
442 p0
= v3_dot( axis
, tri
[0] ),
443 p1
= v3_dot( axis
, tri
[1] ),
444 p2
= v3_dot( axis
, tri
[2] ),
446 e
= vg_maxf(-vg_maxf(p0
,vg_maxf(p1
,p2
)), vg_minf(p0
,vg_minf(p1
,p2
)));
448 if( e
> r
) return 0;
453 * Seperating axis test box vs triangle
455 static int rb_box_triangle_sat( v3f extent
, v3f center
,
456 m4x3f to_local
, v3f tri_src
[3] ){
459 for( int i
=0; i
<3; i
++ ){
460 m4x3_mulv( to_local
, tri_src
[i
], tri
[i
] );
461 v3_sub( tri
[i
], center
, tri
[i
] );
465 v3_sub( tri
[1], tri
[0], f0
);
466 v3_sub( tri
[2], tri
[1], f1
);
467 v3_sub( tri
[0], tri
[2], f2
);
471 v3_cross( (v3f
){1.0f
,0.0f
,0.0f
}, f0
, axis
[0] );
472 v3_cross( (v3f
){1.0f
,0.0f
,0.0f
}, f1
, axis
[1] );
473 v3_cross( (v3f
){1.0f
,0.0f
,0.0f
}, f2
, axis
[2] );
474 v3_cross( (v3f
){0.0f
,1.0f
,0.0f
}, f0
, axis
[3] );
475 v3_cross( (v3f
){0.0f
,1.0f
,0.0f
}, f1
, axis
[4] );
476 v3_cross( (v3f
){0.0f
,1.0f
,0.0f
}, f2
, axis
[5] );
477 v3_cross( (v3f
){0.0f
,0.0f
,1.0f
}, f0
, axis
[6] );
478 v3_cross( (v3f
){0.0f
,0.0f
,1.0f
}, f1
, axis
[7] );
479 v3_cross( (v3f
){0.0f
,0.0f
,1.0f
}, f2
, axis
[8] );
481 for( int i
=0; i
<9; i
++ )
482 if(!rb_box_triangle_interval( extent
, axis
[i
], tri
)) return 0;
485 if(!rb_box_triangle_interval( extent
, (v3f
){1.0f
,0.0f
,0.0f
}, tri
)) return 0;
486 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,1.0f
,0.0f
}, tri
)) return 0;
487 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,0.0f
,1.0f
}, tri
)) return 0;
490 v3_cross( f0
, f1
, n
);
491 if(!rb_box_triangle_interval( extent
, n
, tri
)) return 0;
497 * -----------------------------------------------------------------------------
499 * -----------------------------------------------------------------------------
502 static int rb_manifold_apply_filtered( rb_ct
*man
, int len
){
505 for( int i
=0; i
<len
; i
++ ){
508 if( ct
->type
== k_contact_type_disabled
)
518 * Merge two contacts if they are within radius(r) of eachother
520 static void rb_manifold_contact_weld( rb_ct
*ci
, rb_ct
*cj
, float r
){
521 if( v3_dist2( ci
->co
, cj
->co
) < r
*r
){
522 cj
->type
= k_contact_type_disabled
;
523 ci
->p
= (ci
->p
+ cj
->p
) * 0.5f
;
525 v3_add( ci
->co
, cj
->co
, ci
->co
);
526 v3_muls( ci
->co
, 0.5f
, ci
->co
);
529 v3_sub( ci
->rba
->co
, ci
->co
, delta
);
531 float c0
= v3_dot( ci
->n
, delta
),
532 c1
= v3_dot( cj
->n
, delta
);
534 if( c0
< 0.0f
|| c1
< 0.0f
){
536 ci
->type
= k_contact_type_disabled
;
540 v3_muls( ci
->n
, c0
, n
);
541 v3_muladds( n
, cj
->n
, c1
, n
);
551 static void rb_manifold_filter_joint_edges( rb_ct
*man
, int len
, float r
){
552 for( int i
=0; i
<len
-1; i
++ ){
554 if( ci
->type
!= k_contact_type_edge
)
557 for( int j
=i
+1; j
<len
; j
++ ){
559 if( cj
->type
!= k_contact_type_edge
)
562 rb_manifold_contact_weld( ci
, cj
, r
);
568 * Resolve overlapping pairs
570 static void rb_manifold_filter_pairs( rb_ct
*man
, int len
, float r
){
571 for( int i
=0; i
<len
-1; i
++ ){
575 if( ci
->type
== k_contact_type_disabled
) continue;
577 for( int j
=i
+1; j
<len
; j
++ ){
580 if( cj
->type
== k_contact_type_disabled
) continue;
582 if( v3_dist2( ci
->co
, cj
->co
) < r
*r
){
583 cj
->type
= k_contact_type_disabled
;
584 v3_add( cj
->n
, ci
->n
, ci
->n
);
591 float n
= 1.0f
/((float)similar
+1.0f
);
592 v3_muls( ci
->n
, n
, ci
->n
);
595 if( v3_length2(ci
->n
) < 0.1f
*0.1f
)
596 ci
->type
= k_contact_type_disabled
;
598 v3_normalize( ci
->n
);
604 * Remove contacts that are facing away from A
606 static void rb_manifold_filter_backface( rb_ct
*man
, int len
){
607 for( int i
=0; i
<len
; i
++ ){
609 if( ct
->type
== k_contact_type_disabled
)
613 v3_sub( ct
->co
, ct
->rba
->co
, delta
);
615 if( v3_dot( delta
, ct
->n
) > -0.001f
)
616 ct
->type
= k_contact_type_disabled
;
621 * Filter out duplicate coplanar results. Good for spheres.
623 static void rb_manifold_filter_coplanar( rb_ct
*man
, int len
, float w
){
624 for( int i
=0; i
<len
; i
++ ){
626 if( ci
->type
== k_contact_type_disabled
||
627 ci
->type
== k_contact_type_edge
)
630 float d1
= v3_dot( ci
->co
, ci
->n
);
632 for( int j
=0; j
<len
; j
++ ){
637 if( cj
->type
== k_contact_type_disabled
)
640 float d2
= v3_dot( cj
->co
, ci
->n
),
643 if( fabsf( d
) <= w
){
644 cj
->type
= k_contact_type_disabled
;
650 static void rb_debug_contact( rb_ct
*ct
){
652 v3_muladds( ct
->co
, ct
->n
, 0.05f
, p1
);
654 if( ct
->type
== k_contact_type_default
){
655 vg_line_point( ct
->co
, 0.0125f
, 0xff0000ff );
656 vg_line( ct
->co
, p1
, 0xffffffff );
658 else if( ct
->type
== k_contact_type_edge
){
659 vg_line_point( ct
->co
, 0.0125f
, 0xff00ffc0 );
660 vg_line( ct
->co
, p1
, 0xffffffff );
664 static void rb_solver_reset(void){
665 rb_contact_count
= 0;
668 static rb_ct
*rb_global_ct(void){
669 return rb_contact_buffer
+ rb_contact_count
;
672 static void rb_prepare_contact( rb_ct
*ct
, float timestep
){
673 ct
->bias
= -k_phys_baumgarte
* (timestep
*3600.0f
)
674 * vg_minf( 0.0f
, -ct
->p
+k_penetration_slop
);
676 v3_tangent_basis( ct
->n
, ct
->t
[0], ct
->t
[1] );
677 ct
->norm_impulse
= 0.0f
;
678 ct
->tangent_impulse
[0] = 0.0f
;
679 ct
->tangent_impulse
[1] = 0.0f
;
683 * calculate total move to depenetrate object from contacts.
684 * manifold should belong to ONE object only
686 static void rb_depenetrate( rb_ct
*manifold
, int len
, v3f dt
){
689 for( int j
=0; j
<7; j
++ ){
690 for( int i
=0; i
<len
; i
++ ){
691 rb_ct
*ct
= &manifold
[i
];
693 float resolved_amt
= v3_dot( ct
->n
, dt
),
694 remaining
= (ct
->p
-k_penetration_slop
) - resolved_amt
,
695 apply
= vg_maxf( remaining
, 0.0f
) * 0.4f
;
697 v3_muladds( dt
, ct
->n
, apply
, dt
);
703 * Initializing things like tangent vectors
705 static void rb_presolve_contacts( rb_ct
*buffer
, int len
){
706 for( int i
=0; i
<len
; i
++ ){
707 rb_ct
*ct
= &buffer
[i
];
708 rb_prepare_contact( ct
, k_rb_delta
);
710 v3f ra
, rb
, raCn
, rbCn
, raCt
, rbCt
;
711 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
712 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
713 v3_cross( ra
, ct
->n
, raCn
);
714 v3_cross( rb
, ct
->n
, rbCn
);
716 /* orient inverse inertia tensors */
718 m3x3_mulv( ct
->rba
->iIw
, raCn
, raCnI
);
719 m3x3_mulv( ct
->rbb
->iIw
, rbCn
, rbCnI
);
721 ct
->normal_mass
= ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
722 ct
->normal_mass
+= v3_dot( raCn
, raCnI
);
723 ct
->normal_mass
+= v3_dot( rbCn
, rbCnI
);
724 ct
->normal_mass
= 1.0f
/ct
->normal_mass
;
726 for( int j
=0; j
<2; j
++ ){
728 v3_cross( ct
->t
[j
], ra
, raCt
);
729 v3_cross( ct
->t
[j
], rb
, rbCt
);
730 m3x3_mulv( ct
->rba
->iIw
, raCt
, raCtI
);
731 m3x3_mulv( ct
->rbb
->iIw
, rbCt
, rbCtI
);
733 ct
->tangent_mass
[j
] = ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
734 ct
->tangent_mass
[j
] += v3_dot( raCt
, raCtI
);
735 ct
->tangent_mass
[j
] += v3_dot( rbCt
, rbCtI
);
736 ct
->tangent_mass
[j
] = 1.0f
/ct
->tangent_mass
[j
];
739 rb_debug_contact( ct
);
743 static void rb_contact_restitution( rb_ct
*ct
, float cr
){
745 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
746 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
747 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
749 float v
= v3_dot( rv
, ct
->n
);
757 * One iteration to solve the contact constraint
759 static void rb_solve_contacts( rb_ct
*buf
, int len
){
760 for( int i
=0; i
<len
; i
++ ){
764 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
765 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
766 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
769 for( int j
=0; j
<2; j
++ ){
770 float f
= k_friction
* ct
->norm_impulse
,
771 vt
= v3_dot( rv
, ct
->t
[j
] ),
772 lambda
= ct
->tangent_mass
[j
] * -vt
;
774 float temp
= ct
->tangent_impulse
[j
];
775 ct
->tangent_impulse
[j
] = vg_clampf( temp
+ lambda
, -f
, f
);
776 lambda
= ct
->tangent_impulse
[j
] - temp
;
779 v3_muls( ct
->t
[j
], lambda
, impulse
);
780 rb_linear_impulse( ct
->rba
, ra
, impulse
);
782 v3_muls( ct
->t
[j
], -lambda
, impulse
);
783 rb_linear_impulse( ct
->rbb
, rb
, impulse
);
787 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
788 float vn
= v3_dot( rv
, ct
->n
),
789 lambda
= ct
->normal_mass
* (-vn
+ ct
->bias
);
791 float temp
= ct
->norm_impulse
;
792 ct
->norm_impulse
= vg_maxf( temp
+ lambda
, 0.0f
);
793 lambda
= ct
->norm_impulse
- temp
;
796 v3_muls( ct
->n
, lambda
, impulse
);
797 rb_linear_impulse( ct
->rba
, ra
, impulse
);
799 v3_muls( ct
->n
, -lambda
, impulse
);
800 rb_linear_impulse( ct
->rbb
, rb
, impulse
);