2 * Copyright (C) 2021-2022 Mt.ZERO Software, Harry Godden - All Rights Reserved
6 * Resources: Box2D - Erin Catto
16 VG_STATIC
void rb_tangent_basis( v3f n
, v3f tx
, v3f ty
);
17 VG_STATIC bh_system bh_system_rigidbodies
;
23 * -----------------------------------------------------------------------------
25 * -----------------------------------------------------------------------------
29 k_rb_rate
= (1.0/VG_TIMESTEP_FIXED
),
30 k_rb_delta
= (1.0/k_rb_rate
),
32 k_damp_linear
= 0.1f
, /* scale velocity 1/(1+x) */
33 k_damp_angular
= 0.1f
, /* scale angular 1/(1+x) */
34 k_penetration_slop
= 0.01f
,
35 k_inertia_scale
= 8.0f
,
36 k_phys_baumgarte
= 0.2f
,
41 k_joint_correction
= 0.01f
,
42 k_joint_impulse
= 1.0f
,
43 k_joint_bias
= 0.08f
; /* positional joints */
45 VG_STATIC
void rb_register_cvar(void)
47 VG_VAR_F32( k_limit_bias
, flags
=VG_VAR_CHEAT
);
48 VG_VAR_F32( k_joint_bias
, flags
=VG_VAR_CHEAT
);
49 VG_VAR_F32( k_joint_correction
, flags
=VG_VAR_CHEAT
);
50 VG_VAR_F32( k_joint_impulse
, flags
=VG_VAR_CHEAT
);
54 * -----------------------------------------------------------------------------
55 * structure definitions
56 * -----------------------------------------------------------------------------
59 typedef struct rigidbody rigidbody
;
60 typedef struct rb_object rb_object
;
61 typedef struct contact rb_ct
;
62 typedef struct rb_sphere rb_sphere
;
63 typedef struct rb_capsule rb_capsule
;
64 typedef struct rb_scene rb_scene
;
85 /* inertia model and inverse world tensor */
88 m4x3f to_world
, to_local
;
96 k_rb_shape_sphere
= 1,
97 k_rb_shape_capsule
= 2,
103 struct rb_sphere sphere
;
104 struct rb_capsule capsule
;
105 struct rb_scene scene
;
110 VG_STATIC
struct contact
{
111 rigidbody
*rba
, *rbb
;
114 float p
, bias
, norm_impulse
, tangent_impulse
[2],
115 normal_mass
, tangent_mass
[2];
119 enum contact_type type
;
121 rb_contact_buffer
[256];
122 VG_STATIC
int rb_contact_count
= 0;
124 typedef struct rb_constr_pos rb_constr_pos
;
125 typedef struct rb_constr_swingtwist rb_constr_swingtwist
;
127 struct rb_constr_pos
{
128 rigidbody
*rba
, *rbb
;
132 struct rb_constr_swingtwist
{
133 rigidbody
*rba
, *rbb
;
135 v4f conevx
, conevy
; /* relative to rba */
136 v3f view_offset
, /* relative to rba */
137 coneva
, conevxb
;/* relative to rbb */
139 int tangent_violation
, axis_violation
;
140 v3f axis
, tangent_axis
, tangent_target
, axis_target
;
143 float tangent_mass
, axis_mass
;
146 struct rb_constr_spring
{
151 * -----------------------------------------------------------------------------
153 * -----------------------------------------------------------------------------
156 VG_STATIC
float sphere_volume( float radius
)
158 float r3
= radius
*radius
*radius
;
159 return (4.0f
/3.0f
) * VG_PIf
* r3
;
162 VG_STATIC
void rb_tangent_basis( v3f n
, v3f tx
, v3f ty
)
164 /* Compute tangent basis (box2d) */
165 if( fabsf( n
[0] ) >= 0.57735027f
){
177 v3_cross( n
, tx
, ty
);
181 * -----------------------------------------------------------------------------
183 * -----------------------------------------------------------------------------
186 VG_STATIC
void rb_debug_contact( rb_ct
*ct
)
189 v3_muladds( ct
->co
, ct
->n
, 0.05f
, p1
);
191 if( ct
->type
== k_contact_type_default
){
192 vg_line_pt3( ct
->co
, 0.0125f
, 0xff0000ff );
193 vg_line( ct
->co
, p1
, 0xffffffff );
195 else if( ct
->type
== k_contact_type_edge
){
196 vg_line_pt3( ct
->co
, 0.0125f
, 0xff00ffc0 );
197 vg_line( ct
->co
, p1
, 0xffffffff );
201 VG_STATIC
void debug_sphere( m4x3f m
, float radius
, u32 colour
)
203 v3f ly
= { 0.0f
, 0.0f
, radius
},
204 lx
= { 0.0f
, radius
, 0.0f
},
205 lz
= { 0.0f
, 0.0f
, radius
};
207 for( int i
=0; i
<16; i
++ ){
208 float t
= ((float)(i
+1) * (1.0f
/16.0f
)) * VG_PIf
* 2.0f
,
212 v3f py
= { s
*radius
, 0.0f
, c
*radius
},
213 px
= { s
*radius
, c
*radius
, 0.0f
},
214 pz
= { 0.0f
, s
*radius
, c
*radius
};
216 v3f p0
, p1
, p2
, p3
, p4
, p5
;
217 m4x3_mulv( m
, py
, p0
);
218 m4x3_mulv( m
, ly
, p1
);
219 m4x3_mulv( m
, px
, p2
);
220 m4x3_mulv( m
, lx
, p3
);
221 m4x3_mulv( m
, pz
, p4
);
222 m4x3_mulv( m
, lz
, p5
);
224 vg_line( p0
, p1
, colour
== 0x00? 0xff00ff00: colour
);
225 vg_line( p2
, p3
, colour
== 0x00? 0xff0000ff: colour
);
226 vg_line( p4
, p5
, colour
== 0x00? 0xffff0000: colour
);
234 VG_STATIC
void debug_capsule( m4x3f m
, float radius
, float h
, u32 colour
)
236 v3f ly
= { 0.0f
, 0.0f
, radius
},
237 lx
= { 0.0f
, radius
, 0.0f
},
238 lz
= { 0.0f
, 0.0f
, radius
};
240 float s0
= sinf(0.0f
)*radius
,
241 c0
= cosf(0.0f
)*radius
;
243 v3f p0
, p1
, up
, right
, forward
;
244 m3x3_mulv( m
, (v3f
){0.0f
,1.0f
,0.0f
}, up
);
245 m3x3_mulv( m
, (v3f
){1.0f
,0.0f
,0.0f
}, right
);
246 m3x3_mulv( m
, (v3f
){0.0f
,0.0f
,-1.0f
}, forward
);
247 v3_muladds( m
[3], up
, -h
*0.5f
+radius
, p0
);
248 v3_muladds( m
[3], up
, h
*0.5f
-radius
, p1
);
251 v3_muladds( p0
, right
, radius
, a0
);
252 v3_muladds( p1
, right
, radius
, a1
);
253 v3_muladds( p0
, forward
, radius
, b0
);
254 v3_muladds( p1
, forward
, radius
, b1
);
255 vg_line( a0
, a1
, colour
);
256 vg_line( b0
, b1
, colour
);
258 v3_muladds( p0
, right
, -radius
, a0
);
259 v3_muladds( p1
, right
, -radius
, a1
);
260 v3_muladds( p0
, forward
, -radius
, b0
);
261 v3_muladds( p1
, forward
, -radius
, b1
);
262 vg_line( a0
, a1
, colour
);
263 vg_line( b0
, b1
, colour
);
265 for( int i
=0; i
<16; i
++ ){
266 float t
= ((float)(i
+1) * (1.0f
/16.0f
)) * VG_PIf
* 2.0f
,
270 v3f e0
= { s0
, 0.0f
, c0
},
271 e1
= { s1
, 0.0f
, c1
},
272 e2
= { s0
, c0
, 0.0f
},
273 e3
= { s1
, c1
, 0.0f
},
274 e4
= { 0.0f
, c0
, s0
},
275 e5
= { 0.0f
, c1
, s1
};
277 m3x3_mulv( m
, e0
, e0
);
278 m3x3_mulv( m
, e1
, e1
);
279 m3x3_mulv( m
, e2
, e2
);
280 m3x3_mulv( m
, e3
, e3
);
281 m3x3_mulv( m
, e4
, e4
);
282 m3x3_mulv( m
, e5
, e5
);
284 v3_add( p0
, e0
, a0
);
285 v3_add( p0
, e1
, a1
);
286 v3_add( p1
, e0
, b0
);
287 v3_add( p1
, e1
, b1
);
289 vg_line( a0
, a1
, colour
);
290 vg_line( b0
, b1
, colour
);
293 v3_add( p0
, e2
, a0
);
294 v3_add( p0
, e3
, a1
);
295 v3_add( p0
, e4
, b0
);
296 v3_add( p0
, e5
, b1
);
299 v3_add( p1
, e2
, a0
);
300 v3_add( p1
, e3
, a1
);
301 v3_add( p1
, e4
, b0
);
302 v3_add( p1
, e5
, b1
);
305 vg_line( a0
, a1
, colour
);
306 vg_line( b0
, b1
, colour
);
313 VG_STATIC
void rb_object_debug( rb_object
*obj
, u32 colour
)
315 if( obj
->type
== k_rb_shape_box
){
316 v3f
*box
= obj
->rb
.bbx
;
317 vg_line_boxf_transformed( obj
->rb
.to_world
, obj
->rb
.bbx
, colour
);
319 else if( obj
->type
== k_rb_shape_sphere
){
320 debug_sphere( obj
->rb
.to_world
, obj
->inf
.sphere
.radius
, colour
);
322 else if( obj
->type
== k_rb_shape_capsule
){
324 float h
= obj
->inf
.capsule
.height
,
325 r
= obj
->inf
.capsule
.radius
;
327 debug_capsule( obj
->rb
.to_world
, r
, h
, colour
);
329 else if( obj
->type
== k_rb_shape_scene
){
330 vg_line_boxf( obj
->rb
.bbx
, colour
);
335 * -----------------------------------------------------------------------------
337 * -----------------------------------------------------------------------------
341 * Update world space bounding box based on local one
343 VG_STATIC
void rb_update_bounds( rigidbody
*rb
)
345 box_copy( rb
->bbx
, rb
->bbx_world
);
346 m4x3_transform_aabb( rb
->to_world
, rb
->bbx_world
);
350 * Commit transform to rigidbody. Updates matrices
352 VG_STATIC
void rb_update_transform( rigidbody
*rb
)
354 q_normalize( rb
->q
);
355 q_m3x3( rb
->q
, rb
->to_world
);
356 v3_copy( rb
->co
, rb
->to_world
[3] );
358 m4x3_invert_affine( rb
->to_world
, rb
->to_local
);
361 m3x3_mulv( rb
->to_world
, (v3f
){1.0f
,0.0f
, 0.0f
}, rb
->right
);
362 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,1.0f
, 0.0f
}, rb
->up
);
363 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,0.0f
,-1.0f
}, rb
->forward
);
366 m3x3_mul( rb
->iI
, rb
->to_local
, rb
->iIw
);
367 m3x3_mul( rb
->to_world
, rb
->iIw
, rb
->iIw
);
369 rb_update_bounds( rb
);
373 * Extrapolate rigidbody into a transform based on vg accumulator.
374 * Useful for rendering
376 VG_STATIC
void rb_extrapolate( rigidbody
*rb
, v3f co
, v4f q
)
378 float substep
= vg_clampf( vg
.accumulator
/ k_rb_delta
, 0.0f
, 1.0f
);
380 v3_muladds( rb
->co
, rb
->v
, k_rb_delta
*substep
, co
);
382 if( v3_length2( rb
->w
) > 0.0f
){
385 v3_copy( rb
->w
, axis
);
387 float mag
= v3_length( axis
);
388 v3_divs( axis
, mag
, axis
);
389 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
*substep
);
390 q_mul( rotation
, rb
->q
, q
);
399 * Initialize rigidbody and calculate masses, inertia
401 VG_STATIC
void rb_init_object( rb_object
*obj
)
406 if( obj
->type
== k_rb_shape_box
){
408 v3_sub( obj
->rb
.bbx
[1], obj
->rb
.bbx
[0], dims
);
409 volume
= dims
[0]*dims
[1]*dims
[2];
411 else if( obj
->type
== k_rb_shape_sphere
){
412 volume
= sphere_volume( obj
->inf
.sphere
.radius
);
413 v3_fill( obj
->rb
.bbx
[0], -obj
->inf
.sphere
.radius
);
414 v3_fill( obj
->rb
.bbx
[1], obj
->inf
.sphere
.radius
);
416 else if( obj
->type
== k_rb_shape_capsule
){
417 float r
= obj
->inf
.capsule
.radius
,
418 h
= obj
->inf
.capsule
.height
;
419 volume
= sphere_volume( r
) + VG_PIf
* r
*r
* (h
- r
*2.0f
);
421 v3_fill( obj
->rb
.bbx
[0], -r
);
422 v3_fill( obj
->rb
.bbx
[1], r
);
423 obj
->rb
.bbx
[0][1] = -h
;
424 obj
->rb
.bbx
[1][1] = h
;
426 else if( obj
->type
== k_rb_shape_scene
){
428 box_copy( obj
->inf
.scene
.bh_scene
->nodes
[0].bbx
, obj
->rb
.bbx
);
432 obj
->rb
.inv_mass
= 0.0f
;
433 v3_zero( obj
->rb
.I
);
434 m3x3_zero( obj
->rb
.iI
);
437 float mass
= 2.0f
*volume
;
438 obj
->rb
.inv_mass
= 1.0f
/mass
;
441 v3_sub( obj
->rb
.bbx
[1], obj
->rb
.bbx
[0], extent
);
442 v3_muls( extent
, 0.5f
, extent
);
444 /* local intertia tensor */
445 float scale
= k_inertia_scale
;
446 float ex2
= scale
*extent
[0]*extent
[0],
447 ey2
= scale
*extent
[1]*extent
[1],
448 ez2
= scale
*extent
[2]*extent
[2];
450 obj
->rb
.I
[0] = ((1.0f
/12.0f
) * mass
* (ey2
+ez2
));
451 obj
->rb
.I
[1] = ((1.0f
/12.0f
) * mass
* (ex2
+ez2
));
452 obj
->rb
.I
[2] = ((1.0f
/12.0f
) * mass
* (ex2
+ey2
));
454 m3x3_identity( obj
->rb
.iI
);
455 obj
->rb
.iI
[0][0] = obj
->rb
.I
[0];
456 obj
->rb
.iI
[1][1] = obj
->rb
.I
[1];
457 obj
->rb
.iI
[2][2] = obj
->rb
.I
[2];
458 m3x3_inv( obj
->rb
.iI
, obj
->rb
.iI
);
461 rb_update_transform( &obj
->rb
);
464 VG_STATIC
void rb_iter( rigidbody
*rb
)
466 if( !vg_validf( rb
->v
[0] ) ||
467 !vg_validf( rb
->v
[1] ) ||
468 !vg_validf( rb
->v
[2] ) )
470 vg_fatal_exit_loop( "NaN velocity" );
473 v3f gravity
= { 0.0f
, -9.8f
, 0.0f
};
474 v3_muladds( rb
->v
, gravity
, k_rb_delta
, rb
->v
);
476 /* intergrate velocity */
477 v3_muladds( rb
->co
, rb
->v
, k_rb_delta
, rb
->co
);
478 v3_lerp( rb
->w
, (v3f
){0.0f
,0.0f
,0.0f
}, 0.0025f
, rb
->w
);
480 /* inegrate inertia */
481 if( v3_length2( rb
->w
) > 0.0f
)
485 v3_copy( rb
->w
, axis
);
487 float mag
= v3_length( axis
);
488 v3_divs( axis
, mag
, axis
);
489 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
);
490 q_mul( rotation
, rb
->q
, rb
->q
);
494 v3_muls( rb
->v
, 1.0f
/(1.0f
+k_rb_delta
*k_damp_linear
), rb
->v
);
495 v3_muls( rb
->w
, 1.0f
/(1.0f
+k_rb_delta
*k_damp_angular
), rb
->w
);
500 * -----------------------------------------------------------------------------
501 * Boolean shape overlap functions
502 * -----------------------------------------------------------------------------
506 * Project AABB, and triangle interval onto axis to check if they overlap
508 VG_STATIC
int rb_box_triangle_interval( v3f extent
, v3f axis
, v3f tri
[3] )
512 r
= extent
[0] * fabsf(axis
[0]) +
513 extent
[1] * fabsf(axis
[1]) +
514 extent
[2] * fabsf(axis
[2]),
516 p0
= v3_dot( axis
, tri
[0] ),
517 p1
= v3_dot( axis
, tri
[1] ),
518 p2
= v3_dot( axis
, tri
[2] ),
520 e
= vg_maxf(-vg_maxf(p0
,vg_maxf(p1
,p2
)), vg_minf(p0
,vg_minf(p1
,p2
)));
522 if( e
> r
) return 0;
527 * Seperating axis test box vs triangle
529 VG_STATIC
int rb_box_triangle_sat( v3f extent
, v3f center
,
530 m4x3f to_local
, v3f tri_src
[3] )
534 for( int i
=0; i
<3; i
++ ){
535 m4x3_mulv( to_local
, tri_src
[i
], tri
[i
] );
536 v3_sub( tri
[i
], center
, tri
[i
] );
540 v3_sub( tri
[1], tri
[0], f0
);
541 v3_sub( tri
[2], tri
[1], f1
);
542 v3_sub( tri
[0], tri
[2], f2
);
546 v3_cross( (v3f
){1.0f
,0.0f
,0.0f
}, f0
, axis
[0] );
547 v3_cross( (v3f
){1.0f
,0.0f
,0.0f
}, f1
, axis
[1] );
548 v3_cross( (v3f
){1.0f
,0.0f
,0.0f
}, f2
, axis
[2] );
549 v3_cross( (v3f
){0.0f
,1.0f
,0.0f
}, f0
, axis
[3] );
550 v3_cross( (v3f
){0.0f
,1.0f
,0.0f
}, f1
, axis
[4] );
551 v3_cross( (v3f
){0.0f
,1.0f
,0.0f
}, f2
, axis
[5] );
552 v3_cross( (v3f
){0.0f
,0.0f
,1.0f
}, f0
, axis
[6] );
553 v3_cross( (v3f
){0.0f
,0.0f
,1.0f
}, f1
, axis
[7] );
554 v3_cross( (v3f
){0.0f
,0.0f
,1.0f
}, f2
, axis
[8] );
556 for( int i
=0; i
<9; i
++ )
557 if(!rb_box_triangle_interval( extent
, axis
[i
], tri
)) return 0;
560 if(!rb_box_triangle_interval( extent
, (v3f
){1.0f
,0.0f
,0.0f
}, tri
)) return 0;
561 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,1.0f
,0.0f
}, tri
)) return 0;
562 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,0.0f
,1.0f
}, tri
)) return 0;
565 v3_cross( f0
, f1
, n
);
566 if(!rb_box_triangle_interval( extent
, n
, tri
)) return 0;
572 * -----------------------------------------------------------------------------
574 * -----------------------------------------------------------------------------
577 VG_STATIC
int rb_manifold_apply_filtered( rb_ct
*man
, int len
)
581 for( int i
=0; i
<len
; i
++ ){
584 if( ct
->type
== k_contact_type_disabled
)
594 * Merge two contacts if they are within radius(r) of eachother
596 VG_STATIC
void rb_manifold_contact_weld( rb_ct
*ci
, rb_ct
*cj
, float r
)
598 if( v3_dist2( ci
->co
, cj
->co
) < r
*r
){
599 cj
->type
= k_contact_type_disabled
;
600 ci
->p
= (ci
->p
+ cj
->p
) * 0.5f
;
602 v3_add( ci
->co
, cj
->co
, ci
->co
);
603 v3_muls( ci
->co
, 0.5f
, ci
->co
);
606 v3_sub( ci
->rba
->co
, ci
->co
, delta
);
608 float c0
= v3_dot( ci
->n
, delta
),
609 c1
= v3_dot( cj
->n
, delta
);
611 if( c0
< 0.0f
|| c1
< 0.0f
){
613 ci
->type
= k_contact_type_disabled
;
617 v3_muls( ci
->n
, c0
, n
);
618 v3_muladds( n
, cj
->n
, c1
, n
);
628 VG_STATIC
void rb_manifold_filter_joint_edges( rb_ct
*man
, int len
, float r
)
630 for( int i
=0; i
<len
-1; i
++ ){
632 if( ci
->type
!= k_contact_type_edge
)
635 for( int j
=i
+1; j
<len
; j
++ ){
637 if( cj
->type
!= k_contact_type_edge
)
640 rb_manifold_contact_weld( ci
, cj
, r
);
646 * Resolve overlapping pairs
650 VG_STATIC
void rb_manifold_filter_pairs( rb_ct
*man
, int len
, float r
)
652 for( int i
=0; i
<len
-1; i
++ ){
656 if( ci
->type
== k_contact_type_disabled
) continue;
658 for( int j
=i
+1; j
<len
; j
++ ){
661 if( cj
->type
== k_contact_type_disabled
) continue;
663 if( v3_dist2( ci
->co
, cj
->co
) < r
*r
){
664 cj
->type
= k_contact_type_disabled
;
665 v3_add( cj
->n
, ci
->n
, ci
->n
);
672 float n
= 1.0f
/((float)similar
+1.0f
);
673 v3_muls( ci
->n
, n
, ci
->n
);
676 if( v3_length2(ci
->n
) < 0.1f
*0.1f
)
677 ci
->type
= k_contact_type_disabled
;
679 v3_normalize( ci
->n
);
685 * Remove contacts that are facing away from A
687 VG_STATIC
void rb_manifold_filter_backface( rb_ct
*man
, int len
)
689 for( int i
=0; i
<len
; i
++ ){
691 if( ct
->type
== k_contact_type_disabled
)
695 v3_sub( ct
->co
, ct
->rba
->co
, delta
);
697 if( v3_dot( delta
, ct
->n
) > -0.001f
)
698 ct
->type
= k_contact_type_disabled
;
703 * Filter out duplicate coplanar results. Good for spheres.
705 VG_STATIC
void rb_manifold_filter_coplanar( rb_ct
*man
, int len
, float w
)
707 for( int i
=0; i
<len
; i
++ ){
709 if( ci
->type
== k_contact_type_disabled
||
710 ci
->type
== k_contact_type_edge
)
713 float d1
= v3_dot( ci
->co
, ci
->n
);
715 for( int j
=0; j
<len
; j
++ ){
720 if( cj
->type
== k_contact_type_disabled
)
723 float d2
= v3_dot( cj
->co
, ci
->n
),
726 if( fabsf( d
) <= w
){
727 cj
->type
= k_contact_type_disabled
;
734 * -----------------------------------------------------------------------------
736 * -----------------------------------------------------------------------------
742 * These do not automatically allocate contacts, an appropriately sized
743 * buffer must be supplied. The function returns the size of the manifold
744 * which was generated.
746 * The values set on the contacts are: n, co, p, rba, rbb
750 * By collecting the minimum(time) and maximum(time) pairs of points, we
751 * build a reduced and stable exact manifold.
754 * rx: minimum distance of these points
755 * dx: the delta between the two points
757 * pairs will only ammend these if they are creating a collision
759 typedef struct capsule_manifold capsule_manifold
;
760 struct capsule_manifold
768 * Expand a line manifold with a new pair. t value is the time along segment
769 * on the oriented object which created this pair.
771 VG_STATIC
void rb_capsule_manifold( v3f pa
, v3f pb
, float t
, float r
,
772 capsule_manifold
*manifold
)
775 v3_sub( pa
, pb
, delta
);
777 if( v3_length2(delta
) < r
*r
){
778 if( t
< manifold
->t0
){
779 v3_copy( delta
, manifold
->d0
);
784 if( t
> manifold
->t1
){
785 v3_copy( delta
, manifold
->d1
);
792 VG_STATIC
void rb_capsule_manifold_init( capsule_manifold
*manifold
)
794 manifold
->t0
= INFINITY
;
795 manifold
->t1
= -INFINITY
;
798 VG_STATIC
int rb_capsule__manifold_done( m4x3f mtx
, rb_capsule
*c
,
799 capsule_manifold
*manifold
,
803 v3_muladds( mtx
[3], mtx
[1], -c
->height
*0.5f
+c
->radius
, p0
);
804 v3_muladds( mtx
[3], mtx
[1], c
->height
*0.5f
-c
->radius
, p1
);
807 if( manifold
->t0
<= 1.0f
){
811 v3_muls( p0
, 1.0f
-manifold
->t0
, pa
);
812 v3_muladds( pa
, p1
, manifold
->t0
, pa
);
814 float d
= v3_length( manifold
->d0
);
815 v3_muls( manifold
->d0
, 1.0f
/d
, ct
->n
);
816 v3_muladds( pa
, ct
->n
, -c
->radius
, ct
->co
);
818 ct
->p
= manifold
->r0
- d
;
819 ct
->type
= k_contact_type_default
;
823 if( (manifold
->t1
>= 0.0f
) && (manifold
->t0
!= manifold
->t1
) ){
824 rb_ct
*ct
= buf
+count
;
827 v3_muls( p0
, 1.0f
-manifold
->t1
, pa
);
828 v3_muladds( pa
, p1
, manifold
->t1
, pa
);
830 float d
= v3_length( manifold
->d1
);
831 v3_muls( manifold
->d1
, 1.0f
/d
, ct
->n
);
832 v3_muladds( pa
, ct
->n
, -c
->radius
, ct
->co
);
834 ct
->p
= manifold
->r1
- d
;
835 ct
->type
= k_contact_type_default
;
845 vg_line( buf
[0].co
, buf
[1].co
, 0xff0000ff );
850 VG_STATIC
int rb_capsule_sphere( rb_object
*obja
, rb_object
*objb
, rb_ct
*buf
)
852 rigidbody
*rba
= &obja
->rb
, *rbb
= &objb
->rb
;
853 float h
= obja
->inf
.capsule
.height
,
854 ra
= obja
->inf
.capsule
.radius
,
855 rb
= objb
->inf
.sphere
.radius
;
858 v3_muladds( rba
->co
, rba
->to_world
[1], -h
*0.5f
+ra
, p0
);
859 v3_muladds( rba
->co
, rba
->to_world
[1], h
*0.5f
-ra
, p1
);
862 closest_point_segment( p0
, p1
, rbb
->co
, c
);
863 v3_sub( c
, rbb
->co
, delta
);
865 float d2
= v3_length2(delta
),
872 v3_muls( delta
, 1.0f
/d
, ct
->n
);
876 v3_muladds( c
, ct
->n
, -ra
, p0
);
877 v3_muladds( rbb
->co
, ct
->n
, rb
, p1
);
878 v3_add( p0
, p1
, ct
->co
);
879 v3_muls( ct
->co
, 0.5f
, ct
->co
);
883 ct
->type
= k_contact_type_default
;
891 VG_STATIC
int rb_capsule__capsule( m4x3f mtxA
, rb_capsule
*ca
,
892 m4x3f mtxB
, rb_capsule
*cb
, rb_ct
*buf
)
894 float ha
= ca
->height
,
901 v3_muladds( mtxA
[3], mtxA
[1], -ha
*0.5f
+ra
, p0
);
902 v3_muladds( mtxA
[3], mtxA
[1], ha
*0.5f
-ra
, p1
);
903 v3_muladds( mtxB
[3], mtxB
[1], -hb
*0.5f
+rb
, p2
);
904 v3_muladds( mtxB
[3], mtxB
[1], hb
*0.5f
-rb
, p3
);
906 capsule_manifold manifold
;
907 rb_capsule_manifold_init( &manifold
);
911 closest_segment_segment( p0
, p1
, p2
, p3
, &ta
, &tb
, pa
, pb
);
912 rb_capsule_manifold( pa
, pb
, ta
, r
, &manifold
);
914 ta
= closest_point_segment( p0
, p1
, p2
, pa
);
915 tb
= closest_point_segment( p0
, p1
, p3
, pb
);
916 rb_capsule_manifold( pa
, p2
, ta
, r
, &manifold
);
917 rb_capsule_manifold( pb
, p3
, tb
, r
, &manifold
);
919 closest_point_segment( p2
, p3
, p0
, pa
);
920 closest_point_segment( p2
, p3
, p1
, pb
);
921 rb_capsule_manifold( p0
, pa
, 0.0f
, r
, &manifold
);
922 rb_capsule_manifold( p1
, pb
, 1.0f
, r
, &manifold
);
924 return rb_capsule__manifold_done( mtxA
, ca
, &manifold
, buf
);
929 * Generates up to two contacts; optimised for the most stable manifold
931 VG_STATIC
int rb_capsule_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
933 float h
= rba
->inf
.capsule
.height
,
934 r
= rba
->inf
.capsule
.radius
;
937 * Solving this in symetric local space of the cube saves us some time and a
938 * couple branches when it comes to the quad stage.
941 v3_add( rbb
->bbx
[0], rbb
->bbx
[1], centroid
);
942 v3_muls( centroid
, 0.5f
, centroid
);
945 v3_sub( rbb
->bbx
[0], centroid
, bbx
[0] );
946 v3_sub( rbb
->bbx
[1], centroid
, bbx
[1] );
948 v3f pc
, p0w
, p1w
, p0
, p1
;
949 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+r
, p0w
);
950 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-r
, p1w
);
952 m4x3_mulv( rbb
->to_local
, p0w
, p0
);
953 m4x3_mulv( rbb
->to_local
, p1w
, p1
);
954 v3_sub( p0
, centroid
, p0
);
955 v3_sub( p1
, centroid
, p1
);
956 v3_add( p0
, p1
, pc
);
957 v3_muls( pc
, 0.5f
, pc
);
960 * Finding an appropriate quad to collide lines with
963 v3_div( pc
, bbx
[1], region
);
966 if( (fabsf(region
[0]) > fabsf(region
[1])) &&
967 (fabsf(region
[0]) > fabsf(region
[2])) )
969 float px
= vg_signf(region
[0]) * bbx
[1][0];
970 v3_copy( (v3f
){ px
, bbx
[0][1], bbx
[0][2] }, quad
[0] );
971 v3_copy( (v3f
){ px
, bbx
[1][1], bbx
[0][2] }, quad
[1] );
972 v3_copy( (v3f
){ px
, bbx
[1][1], bbx
[1][2] }, quad
[2] );
973 v3_copy( (v3f
){ px
, bbx
[0][1], bbx
[1][2] }, quad
[3] );
975 else if( fabsf(region
[1]) > fabsf(region
[2]) )
977 float py
= vg_signf(region
[1]) * bbx
[1][1];
978 v3_copy( (v3f
){ bbx
[0][0], py
, bbx
[0][2] }, quad
[0] );
979 v3_copy( (v3f
){ bbx
[1][0], py
, bbx
[0][2] }, quad
[1] );
980 v3_copy( (v3f
){ bbx
[1][0], py
, bbx
[1][2] }, quad
[2] );
981 v3_copy( (v3f
){ bbx
[0][0], py
, bbx
[1][2] }, quad
[3] );
985 float pz
= vg_signf(region
[2]) * bbx
[1][2];
986 v3_copy( (v3f
){ bbx
[0][0], bbx
[0][1], pz
}, quad
[0] );
987 v3_copy( (v3f
){ bbx
[1][0], bbx
[0][1], pz
}, quad
[1] );
988 v3_copy( (v3f
){ bbx
[1][0], bbx
[1][1], pz
}, quad
[2] );
989 v3_copy( (v3f
){ bbx
[0][0], bbx
[1][1], pz
}, quad
[3] );
992 capsule_manifold manifold
;
993 rb_capsule_manifold_init( &manifold
);
996 closest_point_aabb( p0
, bbx
, c0
);
997 closest_point_aabb( p1
, bbx
, c1
);
1000 v3_sub( c0
, p0
, d0
);
1001 v3_sub( c1
, p1
, d1
);
1002 v3_sub( p1
, p0
, da
);
1008 if( v3_dot( da
, d0
) <= 0.01f
)
1009 rb_capsule_manifold( p0
, c0
, 0.0f
, r
, &manifold
);
1011 if( v3_dot( da
, d1
) >= -0.01f
)
1012 rb_capsule_manifold( p1
, c1
, 1.0f
, r
, &manifold
);
1014 for( int i
=0; i
<4; i
++ )
1021 closest_segment_segment( p0
, p1
, quad
[i0
], quad
[i1
], &ta
, &tb
, ca
, cb
);
1022 rb_capsule_manifold( ca
, cb
, ta
, r
, &manifold
);
1026 * Create final contacts based on line manifold
1028 m3x3_mulv( rbb
->to_world
, manifold
.d0
, manifold
.d0
);
1029 m3x3_mulv( rbb
->to_world
, manifold
.d1
, manifold
.d1
);
1036 for( int i
=0; i
<4; i
++ )
1042 v3_add( quad
[i0
], centroid
, q0
);
1043 v3_add( quad
[i1
], centroid
, q1
);
1045 m4x3_mulv( rbb
->to_world
, q0
, q0
);
1046 m4x3_mulv( rbb
->to_world
, q1
, q1
);
1048 vg_line( q0
, q1
, 0xffffffff );
1052 return rb_capsule_manifold_done( rba
, rbb
, &manifold
, buf
);
1056 VG_STATIC
int rb_sphere_box( rb_object
*obja
, rb_object
*objb
, rb_ct
*buf
)
1059 rigidbody
*rba
= &obja
->rb
, *rbb
= &objb
->rb
;
1061 closest_point_obb( rba
->co
, rbb
->bbx
, rbb
->to_world
, rbb
->to_local
, co
);
1062 v3_sub( rba
->co
, co
, delta
);
1064 float d2
= v3_length2(delta
),
1065 r
= obja
->inf
.sphere
.radius
;
1071 if( d2
<= 0.0001f
){
1072 v3_sub( rba
->co
, rbb
->co
, delta
);
1075 * some extra testing is required to find the best axis to push the
1076 * object back outside the box. Since there isnt a clear seperating
1077 * vector already, especially on really high aspect boxes.
1079 float lx
= v3_dot( rbb
->to_world
[0], delta
),
1080 ly
= v3_dot( rbb
->to_world
[1], delta
),
1081 lz
= v3_dot( rbb
->to_world
[2], delta
),
1082 px
= rbb
->bbx
[1][0] - fabsf(lx
),
1083 py
= rbb
->bbx
[1][1] - fabsf(ly
),
1084 pz
= rbb
->bbx
[1][2] - fabsf(lz
);
1086 if( px
< py
&& px
< pz
)
1087 v3_muls( rbb
->to_world
[0], vg_signf(lx
), ct
->n
);
1089 v3_muls( rbb
->to_world
[1], vg_signf(ly
), ct
->n
);
1091 v3_muls( rbb
->to_world
[2], vg_signf(lz
), ct
->n
);
1093 v3_muladds( rba
->co
, ct
->n
, -r
, ct
->co
);
1098 v3_muls( delta
, 1.0f
/d
, ct
->n
);
1100 v3_copy( co
, ct
->co
);
1105 ct
->type
= k_contact_type_default
;
1112 VG_STATIC
int rb_sphere_sphere( rb_object
*obja
, rb_object
*objb
, rb_ct
*buf
)
1114 rigidbody
*rba
= &obja
->rb
, *rbb
= &objb
->rb
;
1116 v3_sub( rba
->co
, rbb
->co
, delta
);
1118 float d2
= v3_length2(delta
),
1119 r
= obja
->inf
.sphere
.radius
+ objb
->inf
.sphere
.radius
;
1122 float d
= sqrtf(d2
);
1125 v3_muls( delta
, 1.0f
/d
, ct
->n
);
1128 v3_muladds( rba
->co
, ct
->n
,-obja
->inf
.sphere
.radius
, p0
);
1129 v3_muladds( rbb
->co
, ct
->n
, objb
->inf
.sphere
.radius
, p1
);
1130 v3_add( p0
, p1
, ct
->co
);
1131 v3_muls( ct
->co
, 0.5f
, ct
->co
);
1132 ct
->type
= k_contact_type_default
;
1142 //#define RIGIDBODY_DYNAMIC_MESH_EDGES
1145 __attribute__ ((deprecated
))
1146 VG_STATIC
int rb_sphere_triangle( rigidbody
*rba
, rigidbody
*rbb
,
1147 v3f tri
[3], rb_ct
*buf
)
1151 #ifdef RIGIDBODY_DYNAMIC_MESH_EDGES
1152 closest_on_triangle_1( rba
->co
, tri
, co
);
1154 enum contact_type type
= closest_on_triangle_1( rba
->co
, tri
, co
);
1157 v3_sub( rba
->co
, co
, delta
);
1159 float d2
= v3_length2( delta
),
1160 r
= rba
->inf
.sphere
.radius
;
1167 v3_sub( tri
[2], tri
[0], ab
);
1168 v3_sub( tri
[1], tri
[0], ac
);
1169 v3_cross( ac
, ab
, tn
);
1170 v3_copy( tn
, ct
->n
);
1172 if( v3_length2( ct
->n
) <= 0.00001f
)
1174 vg_error( "Zero area triangle!\n" );
1178 v3_normalize( ct
->n
);
1180 float d
= sqrtf(d2
);
1182 v3_copy( co
, ct
->co
);
1194 VG_STATIC
int rb_sphere__triangle( m4x3f mtxA
, rb_sphere
*b
,
1195 v3f tri
[3], rb_ct
*buf
)
1198 enum contact_type type
= closest_on_triangle_1( mtxA
[3], tri
, co
);
1200 v3_sub( mtxA
[3], co
, delta
);
1202 float d2
= v3_length2( delta
),
1209 v3_sub( tri
[2], tri
[0], ab
);
1210 v3_sub( tri
[1], tri
[0], ac
);
1211 v3_cross( ac
, ab
, tn
);
1212 v3_copy( tn
, ct
->n
);
1214 if( v3_length2( ct
->n
) <= 0.00001f
){
1215 vg_error( "Zero area triangle!\n" );
1219 v3_normalize( ct
->n
);
1221 float d
= sqrtf(d2
);
1223 v3_copy( co
, ct
->co
);
1232 VG_STATIC
int rb_sphere__scene( m4x3f mtxA
, rb_sphere
*b
,
1233 m4x3f mtxB
, rb_scene
*s
, rb_ct
*buf
)
1235 scene
*sc
= s
->bh_scene
->user
;
1238 bh_iter_init( 0, &it
);
1243 float r
= b
->radius
+ 0.1f
;
1245 v3_sub( mtxA
[3], (v3f
){ r
,r
,r
}, box
[0] );
1246 v3_add( mtxA
[3], (v3f
){ r
,r
,r
}, box
[1] );
1248 while( bh_next( s
->bh_scene
, &it
, box
, &idx
) ){
1249 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
1252 for( int j
=0; j
<3; j
++ )
1253 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
1255 buf
[ count
].element_id
= ptri
[0];
1257 vg_line( tri
[0],tri
[1],0x70ff6000 );
1258 vg_line( tri
[1],tri
[2],0x70ff6000 );
1259 vg_line( tri
[2],tri
[0],0x70ff6000 );
1261 int contact
= rb_sphere__triangle( mtxA
, b
, tri
, &buf
[count
] );
1265 vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
1273 VG_STATIC
int rb_box__scene( m4x3f mtxA
, boxf bbx
,
1274 m4x3f mtxB
, rb_scene
*s
, rb_ct
*buf
)
1277 scene
*sc
= s
->bh_scene
->user
;
1281 v3_sub( bbx
[1], bbx
[0], extent
);
1282 v3_muls( extent
, 0.5f
, extent
);
1283 v3_add( bbx
[0], extent
, center
);
1285 float r
= v3_length(extent
);
1287 v3_fill( world_bbx
[0], -r
);
1288 v3_fill( world_bbx
[1], r
);
1289 for( int i
=0; i
<2; i
++ ){
1290 v3_add( center
, world_bbx
[i
], world_bbx
[i
] );
1291 v3_add( mtxA
[3], world_bbx
[i
], world_bbx
[i
] );
1295 m4x3_invert_affine( mtxA
, to_local
);
1298 bh_iter_init( 0, &it
);
1302 vg_line_boxf( world_bbx
, VG__RED
);
1304 while( bh_next( s
->bh_scene
, &it
, world_bbx
, &idx
) ){
1305 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
1307 for( int j
=0; j
<3; j
++ )
1308 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
1310 if( rb_box_triangle_sat( extent
, center
, to_local
, tri
) ){
1311 vg_line(tri
[0],tri
[1],0xff50ff00 );
1312 vg_line(tri
[1],tri
[2],0xff50ff00 );
1313 vg_line(tri
[2],tri
[0],0xff50ff00 );
1316 vg_line(tri
[0],tri
[1],0xff0000ff );
1317 vg_line(tri
[1],tri
[2],0xff0000ff );
1318 vg_line(tri
[2],tri
[0],0xff0000ff );
1323 v3_sub( tri
[1], tri
[0], v0
);
1324 v3_sub( tri
[2], tri
[0], v1
);
1325 v3_cross( v0
, v1
, n
);
1328 /* find best feature */
1329 float best
= v3_dot( mtxA
[0], n
);
1332 for( int i
=1; i
<3; i
++ ){
1333 float c
= v3_dot( mtxA
[i
], n
);
1335 if( fabsf(c
) > fabsf(best
) ){
1344 float px
= best
> 0.0f
? bbx
[0][0]: bbx
[1][0];
1345 manifold
[0][0] = px
;
1346 manifold
[0][1] = bbx
[0][1];
1347 manifold
[0][2] = bbx
[0][2];
1348 manifold
[1][0] = px
;
1349 manifold
[1][1] = bbx
[1][1];
1350 manifold
[1][2] = bbx
[0][2];
1351 manifold
[2][0] = px
;
1352 manifold
[2][1] = bbx
[1][1];
1353 manifold
[2][2] = bbx
[1][2];
1354 manifold
[3][0] = px
;
1355 manifold
[3][1] = bbx
[0][1];
1356 manifold
[3][2] = bbx
[1][2];
1358 else if( axis
== 1 ){
1359 float py
= best
> 0.0f
? bbx
[0][1]: bbx
[1][1];
1360 manifold
[0][0] = bbx
[0][0];
1361 manifold
[0][1] = py
;
1362 manifold
[0][2] = bbx
[0][2];
1363 manifold
[1][0] = bbx
[1][0];
1364 manifold
[1][1] = py
;
1365 manifold
[1][2] = bbx
[0][2];
1366 manifold
[2][0] = bbx
[1][0];
1367 manifold
[2][1] = py
;
1368 manifold
[2][2] = bbx
[1][2];
1369 manifold
[3][0] = bbx
[0][0];
1370 manifold
[3][1] = py
;
1371 manifold
[3][2] = bbx
[1][2];
1374 float pz
= best
> 0.0f
? bbx
[0][2]: bbx
[1][2];
1375 manifold
[0][0] = bbx
[0][0];
1376 manifold
[0][1] = bbx
[0][1];
1377 manifold
[0][2] = pz
;
1378 manifold
[1][0] = bbx
[1][0];
1379 manifold
[1][1] = bbx
[0][1];
1380 manifold
[1][2] = pz
;
1381 manifold
[2][0] = bbx
[1][0];
1382 manifold
[2][1] = bbx
[1][1];
1383 manifold
[2][2] = pz
;
1384 manifold
[3][0] = bbx
[0][0];
1385 manifold
[3][1] = bbx
[1][1];
1386 manifold
[3][2] = pz
;
1389 for( int j
=0; j
<4; j
++ )
1390 m4x3_mulv( mtxA
, manifold
[j
], manifold
[j
] );
1392 vg_line( manifold
[0], manifold
[1], 0xffffffff );
1393 vg_line( manifold
[1], manifold
[2], 0xffffffff );
1394 vg_line( manifold
[2], manifold
[3], 0xffffffff );
1395 vg_line( manifold
[3], manifold
[0], 0xffffffff );
1397 for( int j
=0; j
<4; j
++ ){
1398 rb_ct
*ct
= buf
+count
;
1400 v3_copy( manifold
[j
], ct
->co
);
1401 v3_copy( n
, ct
->n
);
1403 float l0
= v3_dot( tri
[0], n
),
1404 l1
= v3_dot( manifold
[j
], n
);
1406 ct
->p
= (l0
-l1
)*0.5f
;
1410 ct
->type
= k_contact_type_default
;
1420 scene
*sc
= s
->bh_scene
->user
;
1424 v3_sub( bbx
[1], bbx
[0], extent
);
1425 v3_muls( extent
, 0.5f
, extent
);
1426 v3_add( bbx
[0], extent
, center
);
1428 float r
= v3_length(extent
);
1430 v3_fill( world_bbx
[0], -r
);
1431 v3_fill( world_bbx
[1], r
);
1432 for( int i
=0; i
<2; i
++ ){
1433 v3_add( center
, world_bbx
[i
], world_bbx
[i
] );
1434 v3_add( mtxA
[3], world_bbx
[i
], world_bbx
[i
] );
1438 m4x3_invert_affine( mtxA
, to_local
);
1441 bh_iter_init( 0, &it
);
1445 vg_line_boxf( world_bbx
, VG__RED
);
1447 while( bh_next( s
->bh_scene
, &it
, world_bbx
, &idx
) ){
1448 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
1450 for( int j
=0; j
<3; j
++ )
1451 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
1453 vg_line( tri
[0],tri
[1],VG__BLACK
);
1454 vg_line( tri
[1],tri
[2],VG__BLACK
);
1455 vg_line( tri
[2],tri
[0],VG__BLACK
);
1458 u32 clip_length
= 0;
1464 VG_STATIC
int rb_capsule__triangle( m4x3f mtxA
, rb_capsule
*c
,
1465 v3f tri
[3], rb_ct
*buf
)
1468 v3_muladds( mtxA
[3], mtxA
[1], -c
->height
*0.5f
+c
->radius
, p0w
);
1469 v3_muladds( mtxA
[3], mtxA
[1], c
->height
*0.5f
-c
->radius
, p1w
);
1471 capsule_manifold manifold
;
1472 rb_capsule_manifold_init( &manifold
);
1475 closest_on_triangle_1( p0w
, tri
, c0
);
1476 closest_on_triangle_1( p1w
, tri
, c1
);
1479 v3_sub( c0
, p0w
, d0
);
1480 v3_sub( c1
, p1w
, d1
);
1481 v3_sub( p1w
, p0w
, da
);
1487 if( v3_dot( da
, d0
) <= 0.01f
)
1488 rb_capsule_manifold( p0w
, c0
, 0.0f
, c
->radius
, &manifold
);
1490 if( v3_dot( da
, d1
) >= -0.01f
)
1491 rb_capsule_manifold( p1w
, c1
, 1.0f
, c
->radius
, &manifold
);
1493 for( int i
=0; i
<3; i
++ ){
1499 closest_segment_segment( p0w
, p1w
, tri
[i0
], tri
[i1
], &ta
, &tb
, ca
, cb
);
1500 rb_capsule_manifold( ca
, cb
, ta
, c
->radius
, &manifold
);
1504 v3_sub( tri
[1], tri
[0], v0
);
1505 v3_sub( tri
[2], tri
[0], v1
);
1506 v3_cross( v0
, v1
, n
);
1509 int count
= rb_capsule__manifold_done( mtxA
, c
, &manifold
, buf
);
1510 for( int i
=0; i
<count
; i
++ )
1511 v3_copy( n
, buf
[i
].n
);
1516 /* mtxB is defined only for tradition; it is not used currently */
1517 VG_STATIC
int rb_capsule__scene( m4x3f mtxA
, rb_capsule
*c
,
1518 m4x3f mtxB
, rb_scene
*s
,
1522 bh_iter_init( 0, &it
);
1527 v3_sub( mtxA
[3], (v3f
){ c
->height
, c
->height
, c
->height
}, bbx
[0] );
1528 v3_add( mtxA
[3], (v3f
){ c
->height
, c
->height
, c
->height
}, bbx
[1] );
1530 scene
*sc
= s
->bh_scene
->user
;
1532 while( bh_next( s
->bh_scene
, &it
, bbx
, &idx
) ){
1533 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
1536 for( int j
=0; j
<3; j
++ )
1537 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
1539 buf
[ count
].element_id
= ptri
[0];
1541 int contact
= rb_capsule__triangle( mtxA
, c
, tri
, &buf
[count
] );
1545 vg_warn("Exceeding capsule_vs_scene capacity. Geometry too dense!\n");
1553 VG_STATIC
int rb_global_has_space( void )
1555 if( rb_contact_count
+ 16 > vg_list_size(rb_contact_buffer
) )
1561 VG_STATIC rb_ct
*rb_global_buffer( void )
1563 return &rb_contact_buffer
[ rb_contact_count
];
1567 * -----------------------------------------------------------------------------
1569 * -----------------------------------------------------------------------------
1572 VG_STATIC
void rb_solver_reset(void)
1574 rb_contact_count
= 0;
1577 VG_STATIC rb_ct
*rb_global_ct(void)
1579 return rb_contact_buffer
+ rb_contact_count
;
1582 VG_STATIC
void rb_prepare_contact( rb_ct
*ct
, float timestep
)
1584 ct
->bias
= -0.2f
* (timestep
*3600.0f
)
1585 * vg_minf( 0.0f
, -ct
->p
+k_penetration_slop
);
1587 rb_tangent_basis( ct
->n
, ct
->t
[0], ct
->t
[1] );
1588 ct
->norm_impulse
= 0.0f
;
1589 ct
->tangent_impulse
[0] = 0.0f
;
1590 ct
->tangent_impulse
[1] = 0.0f
;
1593 /* calculate total move. manifold should belong to ONE object only */
1594 VG_STATIC
void rb_depenetrate( rb_ct
*manifold
, int len
, v3f dt
)
1598 for( int j
=0; j
<7; j
++ )
1600 for( int i
=0; i
<len
; i
++ )
1602 struct contact
*ct
= &manifold
[i
];
1604 float resolved_amt
= v3_dot( ct
->n
, dt
),
1605 remaining
= (ct
->p
-k_penetration_slop
) - resolved_amt
,
1606 apply
= vg_maxf( remaining
, 0.0f
) * 0.4f
;
1608 v3_muladds( dt
, ct
->n
, apply
, dt
);
1614 * Initializing things like tangent vectors
1616 VG_STATIC
void rb_presolve_contacts( rb_ct
*buffer
, int len
)
1618 for( int i
=0; i
<len
; i
++ ){
1619 rb_ct
*ct
= &buffer
[i
];
1620 rb_prepare_contact( ct
, k_rb_delta
);
1622 v3f ra
, rb
, raCn
, rbCn
, raCt
, rbCt
;
1623 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
1624 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
1625 v3_cross( ra
, ct
->n
, raCn
);
1626 v3_cross( rb
, ct
->n
, rbCn
);
1628 /* orient inverse inertia tensors */
1630 m3x3_mulv( ct
->rba
->iIw
, raCn
, raCnI
);
1631 m3x3_mulv( ct
->rbb
->iIw
, rbCn
, rbCnI
);
1633 ct
->normal_mass
= ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
1634 ct
->normal_mass
+= v3_dot( raCn
, raCnI
);
1635 ct
->normal_mass
+= v3_dot( rbCn
, rbCnI
);
1636 ct
->normal_mass
= 1.0f
/ct
->normal_mass
;
1638 for( int j
=0; j
<2; j
++ ){
1640 v3_cross( ct
->t
[j
], ra
, raCt
);
1641 v3_cross( ct
->t
[j
], rb
, rbCt
);
1642 m3x3_mulv( ct
->rba
->iIw
, raCt
, raCtI
);
1643 m3x3_mulv( ct
->rbb
->iIw
, rbCt
, rbCtI
);
1645 ct
->tangent_mass
[j
] = ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
1646 ct
->tangent_mass
[j
] += v3_dot( raCt
, raCtI
);
1647 ct
->tangent_mass
[j
] += v3_dot( rbCt
, rbCtI
);
1648 ct
->tangent_mass
[j
] = 1.0f
/ct
->tangent_mass
[j
];
1651 rb_debug_contact( ct
);
1656 * Creates relative contact velocity vector
1658 VG_STATIC
void rb_rcv( rigidbody
*rba
, rigidbody
*rbb
, v3f ra
, v3f rb
, v3f rv
)
1661 v3_cross( rba
->w
, ra
, rva
);
1662 v3_add( rba
->v
, rva
, rva
);
1663 v3_cross( rbb
->w
, rb
, rvb
);
1664 v3_add( rbb
->v
, rvb
, rvb
);
1666 v3_sub( rva
, rvb
, rv
);
1669 VG_STATIC
void rb_contact_restitution( rb_ct
*ct
, float cr
)
1672 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
1673 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
1674 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
1676 float v
= v3_dot( rv
, ct
->n
);
1679 ct
->bias
+= -cr
* v
;
1684 * Apply impulse to object
1686 VG_STATIC
void rb_linear_impulse( rigidbody
*rb
, v3f delta
, v3f impulse
)
1689 v3_muladds( rb
->v
, impulse
, rb
->inv_mass
, rb
->v
);
1691 /* Angular velocity */
1693 v3_cross( delta
, impulse
, wa
);
1695 m3x3_mulv( rb
->iIw
, wa
, wa
);
1696 v3_add( rb
->w
, wa
, rb
->w
);
1700 * One iteration to solve the contact constraint
1702 VG_STATIC
void rb_solve_contacts( rb_ct
*buf
, int len
)
1704 for( int i
=0; i
<len
; i
++ ){
1705 struct contact
*ct
= &buf
[i
];
1708 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
1709 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
1710 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
1713 for( int j
=0; j
<2; j
++ ){
1714 float f
= k_friction
* ct
->norm_impulse
,
1715 vt
= v3_dot( rv
, ct
->t
[j
] ),
1716 lambda
= ct
->tangent_mass
[j
] * -vt
;
1718 float temp
= ct
->tangent_impulse
[j
];
1719 ct
->tangent_impulse
[j
] = vg_clampf( temp
+ lambda
, -f
, f
);
1720 lambda
= ct
->tangent_impulse
[j
] - temp
;
1723 v3_muls( ct
->t
[j
], lambda
, impulse
);
1724 rb_linear_impulse( ct
->rba
, ra
, impulse
);
1726 v3_muls( ct
->t
[j
], -lambda
, impulse
);
1727 rb_linear_impulse( ct
->rbb
, rb
, impulse
);
1731 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
1732 float vn
= v3_dot( rv
, ct
->n
),
1733 lambda
= ct
->normal_mass
* (-vn
+ ct
->bias
);
1735 float temp
= ct
->norm_impulse
;
1736 ct
->norm_impulse
= vg_maxf( temp
+ lambda
, 0.0f
);
1737 lambda
= ct
->norm_impulse
- temp
;
1740 v3_muls( ct
->n
, lambda
, impulse
);
1741 rb_linear_impulse( ct
->rba
, ra
, impulse
);
1743 v3_muls( ct
->n
, -lambda
, impulse
);
1744 rb_linear_impulse( ct
->rbb
, rb
, impulse
);
1749 * -----------------------------------------------------------------------------
1751 * -----------------------------------------------------------------------------
1754 VG_STATIC
void rb_debug_position_constraints( rb_constr_pos
*buffer
, int len
)
1756 for( int i
=0; i
<len
; i
++ ){
1757 rb_constr_pos
*constr
= &buffer
[i
];
1758 rigidbody
*rba
= constr
->rba
, *rbb
= constr
->rbb
;
1761 m3x3_mulv( rba
->to_world
, constr
->lca
, wca
);
1762 m3x3_mulv( rbb
->to_world
, constr
->lcb
, wcb
);
1765 v3_add( wca
, rba
->co
, p0
);
1766 v3_add( wcb
, rbb
->co
, p1
);
1767 vg_line_pt3( p0
, 0.0025f
, 0xff000000 );
1768 vg_line_pt3( p1
, 0.0025f
, 0xffffffff );
1769 vg_line2( p0
, p1
, 0xff000000, 0xffffffff );
1773 VG_STATIC
void rb_presolve_swingtwist_constraints( rb_constr_swingtwist
*buf
,
1778 for( int i
=0; i
<len
; i
++ ){
1779 rb_constr_swingtwist
*st
= &buf
[ i
];
1781 v3f vx
, vy
, va
, vxb
, axis
, center
;
1783 m3x3_mulv( st
->rba
->to_world
, st
->conevx
, vx
);
1784 m3x3_mulv( st
->rbb
->to_world
, st
->conevxb
, vxb
);
1785 m3x3_mulv( st
->rba
->to_world
, st
->conevy
, vy
);
1786 m3x3_mulv( st
->rbb
->to_world
, st
->coneva
, va
);
1787 m4x3_mulv( st
->rba
->to_world
, st
->view_offset
, center
);
1788 v3_cross( vy
, vx
, axis
);
1790 /* Constraint violated ? */
1791 float fx
= v3_dot( vx
, va
), /* projection world */
1792 fy
= v3_dot( vy
, va
),
1793 fn
= v3_dot( va
, axis
),
1795 rx
= st
->conevx
[3], /* elipse radii */
1798 lx
= fx
/rx
, /* projection local (fn==lz) */
1801 st
->tangent_violation
= ((lx
*lx
+ ly
*ly
) > fn
*fn
) || (fn
<= 0.0f
);
1802 if( st
->tangent_violation
){
1803 /* Calculate a good position and the axis to solve on */
1804 v2f closest
, tangent
,
1805 p
= { fx
/fabsf(fn
), fy
/fabsf(fn
) };
1807 closest_point_elipse( p
, (v2f
){rx
,ry
}, closest
);
1808 tangent
[0] = -closest
[1] / (ry
*ry
);
1809 tangent
[1] = closest
[0] / (rx
*rx
);
1810 v2_normalize( tangent
);
1813 v3_muladds( axis
, vx
, closest
[0], v0
);
1814 v3_muladds( v0
, vy
, closest
[1], v0
);
1817 v3_muls( vx
, tangent
[0], v1
);
1818 v3_muladds( v1
, vy
, tangent
[1], v1
);
1820 v3_copy( v0
, st
->tangent_target
);
1821 v3_copy( v1
, st
->tangent_axis
);
1823 /* calculate mass */
1825 m3x3_mulv( st
->rba
->iIw
, st
->tangent_axis
, aIw
);
1826 m3x3_mulv( st
->rbb
->iIw
, st
->tangent_axis
, bIw
);
1827 st
->tangent_mass
= 1.0f
/ (v3_dot( st
->tangent_axis
, aIw
) +
1828 v3_dot( st
->tangent_axis
, bIw
));
1830 float angle
= v3_dot( va
, st
->tangent_target
);
1834 v3_cross( vy
, va
, refaxis
); /* our default rotation */
1835 v3_normalize( refaxis
);
1837 float angle
= v3_dot( refaxis
, vxb
);
1838 st
->axis_violation
= fabsf(angle
) < st
->conet
;
1840 if( st
->axis_violation
){
1842 v3_cross( refaxis
, vxb
, dir_test
);
1844 if( v3_dot(dir_test
, va
) < 0.0f
)
1845 st
->axis_violation
= -st
->axis_violation
;
1847 float newang
= (float)st
->axis_violation
* acosf(st
->conet
-0.0001f
);
1850 v3_cross( va
, refaxis
, refaxis_up
);
1851 v3_muls( refaxis_up
, sinf(newang
), st
->axis_target
);
1852 v3_muladds( st
->axis_target
, refaxis
, -cosf(newang
), st
->axis_target
);
1854 /* calculate mass */
1855 v3_copy( va
, st
->axis
);
1857 m3x3_mulv( st
->rba
->iIw
, st
->axis
, aIw
);
1858 m3x3_mulv( st
->rbb
->iIw
, st
->axis
, bIw
);
1859 st
->axis_mass
= 1.0f
/ (v3_dot( st
->axis
, aIw
) +
1860 v3_dot( st
->axis
, bIw
));
1865 VG_STATIC
void rb_debug_swingtwist_constraints( rb_constr_swingtwist
*buf
,
1870 for( int i
=0; i
<len
; i
++ ){
1871 rb_constr_swingtwist
*st
= &buf
[ i
];
1873 v3f vx
, vxb
, vy
, va
, axis
, center
;
1875 m3x3_mulv( st
->rba
->to_world
, st
->conevx
, vx
);
1876 m3x3_mulv( st
->rbb
->to_world
, st
->conevxb
, vxb
);
1877 m3x3_mulv( st
->rba
->to_world
, st
->conevy
, vy
);
1878 m3x3_mulv( st
->rbb
->to_world
, st
->coneva
, va
);
1879 m4x3_mulv( st
->rba
->to_world
, st
->view_offset
, center
);
1880 v3_cross( vy
, vx
, axis
);
1882 float rx
= st
->conevx
[3], /* elipse radii */
1886 v3_muladds( center
, va
, size
, p1
);
1887 vg_line( center
, p1
, 0xffffffff );
1888 vg_line_pt3( p1
, 0.00025f
, 0xffffffff );
1890 if( st
->tangent_violation
){
1891 v3_muladds( center
, st
->tangent_target
, size
, p0
);
1893 vg_line( center
, p0
, 0xff00ff00 );
1894 vg_line_pt3( p0
, 0.00025f
, 0xff00ff00 );
1895 vg_line( p1
, p0
, 0xff000000 );
1898 for( int x
=0; x
<32; x
++ ){
1899 float t0
= ((float)x
* (1.0f
/32.0f
)) * VG_TAUf
,
1900 t1
= (((float)x
+1.0f
) * (1.0f
/32.0f
)) * VG_TAUf
,
1907 v3_muladds( axis
, vx
, c0
*rx
, v0
);
1908 v3_muladds( v0
, vy
, s0
*ry
, v0
);
1909 v3_muladds( axis
, vx
, c1
*rx
, v1
);
1910 v3_muladds( v1
, vy
, s1
*ry
, v1
);
1915 v3_muladds( center
, v0
, size
, p0
);
1916 v3_muladds( center
, v1
, size
, p1
);
1918 u32 col0r
= fabsf(c0
) * 255.0f
,
1919 col0g
= fabsf(s0
) * 255.0f
,
1920 col1r
= fabsf(c1
) * 255.0f
,
1921 col1g
= fabsf(s1
) * 255.0f
,
1922 col
= st
->tangent_violation
? 0xff0000ff: 0xff000000,
1923 col0
= col
| (col0r
<<16) | (col0g
<< 8),
1924 col1
= col
| (col1r
<<16) | (col1g
<< 8);
1926 vg_line2( center
, p0
, VG__NONE
, col0
);
1927 vg_line2( p0
, p1
, col0
, col1
);
1931 v3_muladds( center
, va
, size
, p0
);
1932 v3_muladds( p0
, vxb
, size
, p1
);
1934 vg_line( p0
, p1
, 0xff0000ff );
1936 if( st
->axis_violation
){
1937 v3_muladds( p0
, st
->axis_target
, size
*1.25f
, p1
);
1938 vg_line( p0
, p1
, 0xffffff00 );
1939 vg_line_pt3( p1
, 0.0025f
, 0xffffff80 );
1943 v3_cross( vy
, va
, refaxis
); /* our default rotation */
1944 v3_normalize( refaxis
);
1946 v3_cross( va
, refaxis
, refaxis_up
);
1947 float newang
= acosf(st
->conet
-0.0001f
);
1949 v3_muladds( p0
, refaxis_up
, sinf(newang
)*size
, p1
);
1950 v3_muladds( p1
, refaxis
, -cosf(newang
)*size
, p1
);
1951 vg_line( p0
, p1
, 0xff000000 );
1953 v3_muladds( p0
, refaxis_up
, sinf(-newang
)*size
, p1
);
1954 v3_muladds( p1
, refaxis
, -cosf(-newang
)*size
, p1
);
1955 vg_line( p0
, p1
, 0xff404040 );
1960 * Solve a list of positional constraints
1962 VG_STATIC
void rb_solve_position_constraints( rb_constr_pos
*buf
, int len
)
1964 for( int i
=0; i
<len
; i
++ ){
1965 rb_constr_pos
*constr
= &buf
[i
];
1966 rigidbody
*rba
= constr
->rba
, *rbb
= constr
->rbb
;
1969 m3x3_mulv( rba
->to_world
, constr
->lca
, wa
);
1970 m3x3_mulv( rbb
->to_world
, constr
->lcb
, wb
);
1972 m3x3f ssra
, ssrat
, ssrb
, ssrbt
;
1974 m3x3_skew_symetric( ssrat
, wa
);
1975 m3x3_skew_symetric( ssrbt
, wb
);
1976 m3x3_transpose( ssrat
, ssra
);
1977 m3x3_transpose( ssrbt
, ssrb
);
1979 v3f b
, b_wa
, b_wb
, b_a
, b_b
;
1980 m3x3_mulv( ssra
, rba
->w
, b_wa
);
1981 m3x3_mulv( ssrb
, rbb
->w
, b_wb
);
1982 v3_add( rba
->v
, b_wa
, b
);
1983 v3_sub( b
, rbb
->v
, b
);
1984 v3_sub( b
, b_wb
, b
);
1985 v3_muls( b
, -1.0f
, b
);
1988 m3x3_diagonal( invMa
, rba
->inv_mass
);
1989 m3x3_diagonal( invMb
, rbb
->inv_mass
);
1992 m3x3_mul( ssra
, rba
->iIw
, ia
);
1993 m3x3_mul( ia
, ssrat
, ia
);
1994 m3x3_mul( ssrb
, rbb
->iIw
, ib
);
1995 m3x3_mul( ib
, ssrbt
, ib
);
1998 m3x3_add( invMa
, ia
, cma
);
1999 m3x3_add( invMb
, ib
, cmb
);
2002 m3x3_add( cma
, cmb
, A
);
2004 /* Solve Ax = b ( A^-1*b = x ) */
2007 m3x3_inv( A
, invA
);
2008 m3x3_mulv( invA
, b
, impulse
);
2010 v3f delta_va
, delta_wa
, delta_vb
, delta_wb
;
2012 m3x3_mul( rba
->iIw
, ssrat
, iwa
);
2013 m3x3_mul( rbb
->iIw
, ssrbt
, iwb
);
2015 m3x3_mulv( invMa
, impulse
, delta_va
);
2016 m3x3_mulv( invMb
, impulse
, delta_vb
);
2017 m3x3_mulv( iwa
, impulse
, delta_wa
);
2018 m3x3_mulv( iwb
, impulse
, delta_wb
);
2020 v3_add( rba
->v
, delta_va
, rba
->v
);
2021 v3_add( rba
->w
, delta_wa
, rba
->w
);
2022 v3_sub( rbb
->v
, delta_vb
, rbb
->v
);
2023 v3_sub( rbb
->w
, delta_wb
, rbb
->w
);
2027 VG_STATIC
void rb_solve_swingtwist_constraints( rb_constr_swingtwist
*buf
,
2032 for( int i
=0; i
<len
; i
++ ){
2033 rb_constr_swingtwist
*st
= &buf
[ i
];
2035 if( !st
->axis_violation
)
2038 float rv
= v3_dot( st
->axis
, st
->rbb
->w
) -
2039 v3_dot( st
->axis
, st
->rba
->w
);
2041 if( rv
* (float)st
->axis_violation
> 0.0f
)
2044 v3f impulse
, wa
, wb
;
2045 v3_muls( st
->axis
, rv
*st
->axis_mass
, impulse
);
2046 m3x3_mulv( st
->rba
->iIw
, impulse
, wa
);
2047 v3_add( st
->rba
->w
, wa
, st
->rba
->w
);
2049 v3_muls( impulse
, -1.0f
, impulse
);
2050 m3x3_mulv( st
->rbb
->iIw
, impulse
, wb
);
2051 v3_add( st
->rbb
->w
, wb
, st
->rbb
->w
);
2053 float rv2
= v3_dot( st
->axis
, st
->rbb
->w
) -
2054 v3_dot( st
->axis
, st
->rba
->w
);
2057 for( int i
=0; i
<len
; i
++ ){
2058 rb_constr_swingtwist
*st
= &buf
[ i
];
2060 if( !st
->tangent_violation
)
2063 float rv
= v3_dot( st
->tangent_axis
, st
->rbb
->w
) -
2064 v3_dot( st
->tangent_axis
, st
->rba
->w
);
2069 v3f impulse
, wa
, wb
;
2070 v3_muls( st
->tangent_axis
, rv
*st
->tangent_mass
, impulse
);
2071 m3x3_mulv( st
->rba
->iIw
, impulse
, wa
);
2072 v3_add( st
->rba
->w
, wa
, st
->rba
->w
);
2074 v3_muls( impulse
, -1.0f
, impulse
);
2075 m3x3_mulv( st
->rbb
->iIw
, impulse
, wb
);
2076 v3_add( st
->rbb
->w
, wb
, st
->rbb
->w
);
2078 float rv2
= v3_dot( st
->tangent_axis
, st
->rbb
->w
) -
2079 v3_dot( st
->tangent_axis
, st
->rba
->w
);
2083 VG_STATIC
void rb_solve_constr_angle( rigidbody
*rba
, rigidbody
*rbb
,
2086 m3x3f ssra
, ssrb
, ssrat
, ssrbt
;
2089 m3x3_skew_symetric( ssrat
, ra
);
2090 m3x3_skew_symetric( ssrbt
, rb
);
2091 m3x3_transpose( ssrat
, ssra
);
2092 m3x3_transpose( ssrbt
, ssrb
);
2094 m3x3_mul( ssra
, rba
->iIw
, cma
);
2095 m3x3_mul( cma
, ssrat
, cma
);
2096 m3x3_mul( ssrb
, rbb
->iIw
, cmb
);
2097 m3x3_mul( cmb
, ssrbt
, cmb
);
2100 m3x3_add( cma
, cmb
, A
);
2101 m3x3_inv( A
, invA
);
2104 m3x3_mulv( ssra
, rba
->w
, b_wa
);
2105 m3x3_mulv( ssrb
, rbb
->w
, b_wb
);
2106 v3_add( b_wa
, b_wb
, b
);
2110 m3x3_mulv( invA
, b
, impulse
);
2112 v3f delta_wa
, delta_wb
;
2114 m3x3_mul( rba
->iIw
, ssrat
, iwa
);
2115 m3x3_mul( rbb
->iIw
, ssrbt
, iwb
);
2116 m3x3_mulv( iwa
, impulse
, delta_wa
);
2117 m3x3_mulv( iwb
, impulse
, delta_wb
);
2118 v3_add( rba
->w
, delta_wa
, rba
->w
);
2119 v3_sub( rbb
->w
, delta_wb
, rbb
->w
);
2123 * Correct position constraint drift errors
2124 * [ 0.0 <= amt <= 1.0 ]: the correction amount
2126 VG_STATIC
void rb_correct_position_constraints( rb_constr_pos
*buf
, int len
,
2129 for( int i
=0; i
<len
; i
++ ){
2130 rb_constr_pos
*constr
= &buf
[i
];
2131 rigidbody
*rba
= constr
->rba
, *rbb
= constr
->rbb
;
2134 m3x3_mulv( rba
->to_world
, constr
->lca
, p0
);
2135 m3x3_mulv( rbb
->to_world
, constr
->lcb
, p1
);
2136 v3_add( rba
->co
, p0
, p0
);
2137 v3_add( rbb
->co
, p1
, p1
);
2138 v3_sub( p1
, p0
, d
);
2140 v3_muladds( rbb
->co
, d
, -1.0f
* amt
, rbb
->co
);
2141 rb_update_transform( rbb
);
2145 VG_STATIC
void rb_correct_swingtwist_constraints( rb_constr_swingtwist
*buf
,
2146 int len
, float amt
)
2148 for( int i
=0; i
<len
; i
++ ){
2149 rb_constr_swingtwist
*st
= &buf
[i
];
2151 if( !st
->tangent_violation
)
2155 m3x3_mulv( st
->rbb
->to_world
, st
->coneva
, va
);
2157 float angle
= v3_dot( va
, st
->tangent_target
);
2159 if( fabsf(angle
) < 0.9999f
){
2161 v3_cross( va
, st
->tangent_target
, axis
);
2164 q_axis_angle( correction
, axis
, acosf(angle
) * amt
);
2165 q_mul( correction
, st
->rbb
->q
, st
->rbb
->q
);
2166 rb_update_transform( st
->rbb
);
2170 for( int i
=0; i
<len
; i
++ ){
2171 rb_constr_swingtwist
*st
= &buf
[i
];
2173 if( !st
->axis_violation
)
2177 m3x3_mulv( st
->rbb
->to_world
, st
->conevxb
, vxb
);
2179 float angle
= v3_dot( vxb
, st
->axis_target
);
2181 if( fabsf(angle
) < 0.9999f
){
2183 v3_cross( vxb
, st
->axis_target
, axis
);
2186 q_axis_angle( correction
, axis
, acosf(angle
) * amt
);
2187 q_mul( correction
, st
->rbb
->q
, st
->rbb
->q
);
2188 rb_update_transform( st
->rbb
);
2193 VG_STATIC
void rb_correct_contact_constraints( rb_ct
*buf
, int len
, float amt
)
2195 for( int i
=0; i
<len
; i
++ ){
2196 rb_ct
*ct
= &buf
[i
];
2197 rigidbody
*rba
= ct
->rba
,
2200 float mass_total
= 1.0f
/ (rba
->inv_mass
+ rbb
->inv_mass
);
2202 v3_muladds( rba
->co
, ct
->n
, -mass_total
* rba
->inv_mass
, rba
->co
);
2203 v3_muladds( rbb
->co
, ct
->n
, mass_total
* rbb
->inv_mass
, rbb
->co
);
2212 VG_STATIC
void rb_effect_simple_bouyency( rigidbody
*ra
, v4f plane
,
2213 float amt
, float drag
)
2216 float depth
= v3_dot( plane
, ra
->co
) - plane
[3],
2217 lambda
= vg_clampf( -depth
, 0.0f
, 1.0f
) * amt
;
2219 v3_muladds( ra
->v
, plane
, lambda
* k_rb_delta
, ra
->v
);
2222 v3_muls( ra
->v
, 1.0f
-(drag
*k_rb_delta
), ra
->v
);
2225 /* apply a spring&dampener force to match ra(worldspace) on rigidbody, to
2228 VG_STATIC
void rb_effect_spring_target_vector( rigidbody
*rba
, v3f ra
, v3f rt
,
2229 float spring
, float dampening
,
2232 float d
= v3_dot( rt
, ra
);
2233 float a
= vg_signf( d
) * acosf( vg_clampf( d
, -1.0f
, 1.0f
) );
2236 v3_cross( rt
, ra
, axis
);
2238 float Fs
= -a
* spring
,
2239 Fd
= -v3_dot( rba
->w
, axis
) * dampening
;
2241 v3_muladds( rba
->w
, axis
, (Fs
+Fd
) * timestep
, rba
->w
);
2244 #endif /* RIGIDBODY_H */