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
;
40 k_joint_correction
= 0.01f
,
41 k_joint_impulse
= 1.0f
,
42 k_joint_bias
= 0.08f
; /* positional joints */
44 VG_STATIC
void rb_register_cvar(void)
46 vg_var_push( (struct vg_var
){
47 .name
= "k_limit_bias", .data
= &k_limit_bias
,
48 .data_type
= k_var_dtype_f32
, .opt_f32
= {.clamp
= 0}, .persistent
= 1
51 vg_var_push( (struct vg_var
){
52 .name
= "k_joint_bias", .data
= &k_joint_bias
,
53 .data_type
= k_var_dtype_f32
, .opt_f32
= {.clamp
= 0}, .persistent
= 1
56 vg_var_push( (struct vg_var
){
57 .name
= "k_joint_correction", .data
= &k_joint_correction
,
58 .data_type
= k_var_dtype_f32
, .opt_f32
= {.clamp
= 0}, .persistent
= 1
61 vg_var_push( (struct vg_var
){
62 .name
= "k_joint_impulse", .data
= &k_joint_impulse
,
63 .data_type
= k_var_dtype_f32
, .opt_f32
= {.clamp
= 0}, .persistent
= 1
68 * -----------------------------------------------------------------------------
69 * structure definitions
70 * -----------------------------------------------------------------------------
73 typedef struct rigidbody rigidbody
;
74 typedef struct contact rb_ct
;
84 k_rb_shape_sphere
= 1,
85 k_rb_shape_capsule
= 2,
100 float height
, radius
;
112 v3f right
, up
, forward
;
119 /* inertia model and inverse world tensor */
123 m4x3f to_world
, to_local
;
126 VG_STATIC
struct contact
128 rigidbody
*rba
, *rbb
;
131 float p
, bias
, norm_impulse
, tangent_impulse
[2],
132 normal_mass
, tangent_mass
[2];
136 enum contact_type type
;
138 rb_contact_buffer
[256];
139 VG_STATIC
int rb_contact_count
= 0;
141 typedef struct rb_constr_pos rb_constr_pos
;
142 typedef struct rb_constr_swingtwist rb_constr_swingtwist
;
146 rigidbody
*rba
, *rbb
;
150 struct rb_constr_swingtwist
152 rigidbody
*rba
, *rbb
;
154 v4f conevx
, conevy
; /* relative to rba */
155 v3f view_offset
, /* relative to rba */
156 coneva
, conevxb
;/* relative to rbb */
158 int tangent_violation
, axis_violation
;
159 v3f axis
, tangent_axis
, tangent_target
, axis_target
;
162 float tangent_mass
, axis_mass
;
166 * -----------------------------------------------------------------------------
168 * -----------------------------------------------------------------------------
171 VG_STATIC
float sphere_volume( float radius
)
173 float r3
= radius
*radius
*radius
;
174 return (4.0f
/3.0f
) * VG_PIf
* r3
;
177 VG_STATIC
void rb_tangent_basis( v3f n
, v3f tx
, v3f ty
)
179 /* Compute tangent basis (box2d) */
180 if( fabsf( n
[0] ) >= 0.57735027f
)
194 v3_cross( n
, tx
, ty
);
198 * -----------------------------------------------------------------------------
200 * -----------------------------------------------------------------------------
203 VG_STATIC
void rb_debug_contact( rb_ct
*ct
)
205 if( ct
->type
!= k_contact_type_disabled
)
208 v3_muladds( ct
->co
, ct
->n
, 0.05f
, p1
);
209 vg_line_pt3( ct
->co
, 0.0025f
, 0xff0000ff );
210 vg_line( ct
->co
, p1
, 0xffffffff );
214 VG_STATIC
void debug_sphere( m4x3f m
, float radius
, u32 colour
)
216 v3f ly
= { 0.0f
, 0.0f
, radius
},
217 lx
= { 0.0f
, radius
, 0.0f
},
218 lz
= { 0.0f
, 0.0f
, radius
};
220 for( int i
=0; i
<16; i
++ )
222 float t
= ((float)(i
+1) * (1.0f
/16.0f
)) * VG_PIf
* 2.0f
,
226 v3f py
= { s
*radius
, 0.0f
, c
*radius
},
227 px
= { s
*radius
, c
*radius
, 0.0f
},
228 pz
= { 0.0f
, s
*radius
, c
*radius
};
230 v3f p0
, p1
, p2
, p3
, p4
, p5
;
231 m4x3_mulv( m
, py
, p0
);
232 m4x3_mulv( m
, ly
, p1
);
233 m4x3_mulv( m
, px
, p2
);
234 m4x3_mulv( m
, lx
, p3
);
235 m4x3_mulv( m
, pz
, p4
);
236 m4x3_mulv( m
, lz
, p5
);
238 vg_line( p0
, p1
, colour
== 0x00? 0xff00ff00: colour
);
239 vg_line( p2
, p3
, colour
== 0x00? 0xff0000ff: colour
);
240 vg_line( p4
, p5
, colour
== 0x00? 0xffff0000: colour
);
248 VG_STATIC
void debug_capsule( m4x3f m
, float radius
, float h
, u32 colour
)
250 v3f ly
= { 0.0f
, 0.0f
, radius
},
251 lx
= { 0.0f
, radius
, 0.0f
},
252 lz
= { 0.0f
, 0.0f
, radius
};
254 float s0
= sinf(0.0f
)*radius
,
255 c0
= cosf(0.0f
)*radius
;
257 v3f p0
, p1
, up
, right
, forward
;
258 m3x3_mulv( m
, (v3f
){0.0f
,1.0f
,0.0f
}, up
);
259 m3x3_mulv( m
, (v3f
){1.0f
,0.0f
,0.0f
}, right
);
260 m3x3_mulv( m
, (v3f
){0.0f
,0.0f
,-1.0f
}, forward
);
261 v3_muladds( m
[3], up
, -h
*0.5f
+radius
, p0
);
262 v3_muladds( m
[3], up
, h
*0.5f
-radius
, p1
);
265 v3_muladds( p0
, right
, radius
, a0
);
266 v3_muladds( p1
, right
, radius
, a1
);
267 v3_muladds( p0
, forward
, radius
, b0
);
268 v3_muladds( p1
, forward
, radius
, b1
);
269 vg_line( a0
, a1
, colour
);
270 vg_line( b0
, b1
, colour
);
272 v3_muladds( p0
, right
, -radius
, a0
);
273 v3_muladds( p1
, right
, -radius
, a1
);
274 v3_muladds( p0
, forward
, -radius
, b0
);
275 v3_muladds( p1
, forward
, -radius
, b1
);
276 vg_line( a0
, a1
, colour
);
277 vg_line( b0
, b1
, colour
);
279 for( int i
=0; i
<16; i
++ )
281 float t
= ((float)(i
+1) * (1.0f
/16.0f
)) * VG_PIf
* 2.0f
,
285 v3f e0
= { s0
, 0.0f
, c0
},
286 e1
= { s1
, 0.0f
, c1
},
287 e2
= { s0
, c0
, 0.0f
},
288 e3
= { s1
, c1
, 0.0f
},
289 e4
= { 0.0f
, c0
, s0
},
290 e5
= { 0.0f
, c1
, s1
};
292 m3x3_mulv( m
, e0
, e0
);
293 m3x3_mulv( m
, e1
, e1
);
294 m3x3_mulv( m
, e2
, e2
);
295 m3x3_mulv( m
, e3
, e3
);
296 m3x3_mulv( m
, e4
, e4
);
297 m3x3_mulv( m
, e5
, e5
);
299 v3_add( p0
, e0
, a0
);
300 v3_add( p0
, e1
, a1
);
301 v3_add( p1
, e0
, b0
);
302 v3_add( p1
, e1
, b1
);
304 vg_line( a0
, a1
, colour
);
305 vg_line( b0
, b1
, colour
);
309 v3_add( p0
, e2
, a0
);
310 v3_add( p0
, e3
, a1
);
311 v3_add( p0
, e4
, b0
);
312 v3_add( p0
, e5
, b1
);
316 v3_add( p1
, e2
, a0
);
317 v3_add( p1
, e3
, a1
);
318 v3_add( p1
, e4
, b0
);
319 v3_add( p1
, e5
, b1
);
322 vg_line( a0
, a1
, colour
);
323 vg_line( b0
, b1
, colour
);
330 VG_STATIC
void rb_debug( rigidbody
*rb
, u32 colour
)
332 if( rb
->type
== k_rb_shape_box
)
335 vg_line_boxf_transformed( rb
->to_world
, rb
->bbx
, colour
);
337 else if( rb
->type
== k_rb_shape_sphere
)
339 debug_sphere( rb
->to_world
, rb
->inf
.sphere
.radius
, colour
);
341 else if( rb
->type
== k_rb_shape_capsule
)
344 float h
= rb
->inf
.capsule
.height
,
345 r
= rb
->inf
.capsule
.radius
;
347 debug_capsule( rb
->to_world
, r
, h
, colour
);
349 else if( rb
->type
== k_rb_shape_scene
)
351 vg_line_boxf( rb
->bbx
, colour
);
356 * -----------------------------------------------------------------------------
358 * -----------------------------------------------------------------------------
362 * Update world space bounding box based on local one
364 VG_STATIC
void rb_update_bounds( rigidbody
*rb
)
366 box_copy( rb
->bbx
, rb
->bbx_world
);
367 m4x3_transform_aabb( rb
->to_world
, rb
->bbx_world
);
371 * Commit transform to rigidbody. Updates matrices
373 VG_STATIC
void rb_update_transform( rigidbody
*rb
)
375 q_normalize( rb
->q
);
376 q_m3x3( rb
->q
, rb
->to_world
);
377 v3_copy( rb
->co
, rb
->to_world
[3] );
379 m4x3_invert_affine( rb
->to_world
, rb
->to_local
);
381 m3x3_mulv( rb
->to_world
, (v3f
){1.0f
,0.0f
, 0.0f
}, rb
->right
);
382 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,1.0f
, 0.0f
}, rb
->up
);
383 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,0.0f
,-1.0f
}, rb
->forward
);
385 m3x3_mul( rb
->iI
, rb
->to_local
, rb
->iIw
);
386 m3x3_mul( rb
->to_world
, rb
->iIw
, rb
->iIw
);
388 rb_update_bounds( rb
);
392 * Extrapolate rigidbody into a transform based on vg accumulator.
393 * Useful for rendering
395 VG_STATIC
void rb_extrapolate_transform( rigidbody
*rb
, m4x3f transform
)
397 float substep
= vg_clampf( vg
.accumulator
/ k_rb_delta
, 0.0f
, 1.0f
);
402 v3_muladds( rb
->co
, rb
->v
, k_rb_delta
*substep
, co
);
404 if( v3_length2( rb
->w
) > 0.0f
)
408 v3_copy( rb
->w
, axis
);
410 float mag
= v3_length( axis
);
411 v3_divs( axis
, mag
, axis
);
412 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
*substep
);
413 q_mul( rotation
, rb
->q
, q
);
421 q_m3x3( q
, transform
);
422 v3_copy( co
, transform
[3] );
426 * Initialize rigidbody and calculate masses, inertia
428 VG_STATIC
void rb_init( rigidbody
*rb
)
432 if( rb
->type
== k_rb_shape_box
)
435 v3_sub( rb
->bbx
[1], rb
->bbx
[0], dims
);
436 volume
= dims
[0]*dims
[1]*dims
[2];
438 else if( rb
->type
== k_rb_shape_sphere
)
440 volume
= sphere_volume( rb
->inf
.sphere
.radius
);
441 v3_fill( rb
->bbx
[0], -rb
->inf
.sphere
.radius
);
442 v3_fill( rb
->bbx
[1], rb
->inf
.sphere
.radius
);
444 else if( rb
->type
== k_rb_shape_capsule
)
446 float r
= rb
->inf
.capsule
.radius
,
447 h
= rb
->inf
.capsule
.height
;
448 volume
= sphere_volume( r
) + VG_PIf
* r
*r
* (h
- r
*2.0f
);
450 v3_fill( rb
->bbx
[0], -r
);
451 v3_fill( rb
->bbx
[1], r
);
455 else if( rb
->type
== k_rb_shape_scene
)
458 box_copy( rb
->inf
.scene
.bh_scene
->nodes
[0].bbx
, rb
->bbx
);
469 float mass
= 2.0f
*volume
;
470 rb
->inv_mass
= 1.0f
/mass
;
473 v3_sub( rb
->bbx
[1], rb
->bbx
[0], extent
);
474 v3_muls( extent
, 0.5f
, extent
);
476 /* local intertia tensor */
477 float scale
= k_inertia_scale
;
478 float ex2
= scale
*extent
[0]*extent
[0],
479 ey2
= scale
*extent
[1]*extent
[1],
480 ez2
= scale
*extent
[2]*extent
[2];
482 rb
->I
[0] = ((1.0f
/12.0f
) * mass
* (ey2
+ez2
));
483 rb
->I
[1] = ((1.0f
/12.0f
) * mass
* (ex2
+ez2
));
484 rb
->I
[2] = ((1.0f
/12.0f
) * mass
* (ex2
+ey2
));
486 m3x3_identity( rb
->iI
);
487 rb
->iI
[0][0] = rb
->I
[0];
488 rb
->iI
[1][1] = rb
->I
[1];
489 rb
->iI
[2][2] = rb
->I
[2];
490 m3x3_inv( rb
->iI
, rb
->iI
);
496 rb_update_transform( rb
);
499 VG_STATIC
void rb_iter( rigidbody
*rb
)
501 if( !vg_validf( rb
->v
[0] ) ||
502 !vg_validf( rb
->v
[1] ) ||
503 !vg_validf( rb
->v
[2] ) )
505 vg_fatal_exit_loop( "NaN velocity" );
508 v3f gravity
= { 0.0f
, -9.8f
, 0.0f
};
509 v3_muladds( rb
->v
, gravity
, k_rb_delta
, rb
->v
);
511 /* intergrate velocity */
512 v3_muladds( rb
->co
, rb
->v
, k_rb_delta
, rb
->co
);
513 v3_lerp( rb
->w
, (v3f
){0.0f
,0.0f
,0.0f
}, 0.0025f
, rb
->w
);
515 /* inegrate inertia */
516 if( v3_length2( rb
->w
) > 0.0f
)
520 v3_copy( rb
->w
, axis
);
522 float mag
= v3_length( axis
);
523 v3_divs( axis
, mag
, axis
);
524 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
);
525 q_mul( rotation
, rb
->q
, rb
->q
);
529 v3_muls( rb
->v
, 1.0f
/(1.0f
+k_rb_delta
*k_damp_linear
), rb
->v
);
530 v3_muls( rb
->w
, 1.0f
/(1.0f
+k_rb_delta
*k_damp_angular
), rb
->w
);
535 * -----------------------------------------------------------------------------
536 * Boolean shape overlap functions
537 * -----------------------------------------------------------------------------
541 * Project AABB, and triangle interval onto axis to check if they overlap
543 VG_STATIC
int rb_box_triangle_interval( v3f extent
, v3f axis
, v3f tri
[3] )
547 r
= extent
[0] * fabsf(axis
[0]) +
548 extent
[1] * fabsf(axis
[1]) +
549 extent
[2] * fabsf(axis
[2]),
551 p0
= v3_dot( axis
, tri
[0] ),
552 p1
= v3_dot( axis
, tri
[1] ),
553 p2
= v3_dot( axis
, tri
[2] ),
555 e
= vg_maxf(-vg_maxf(p0
,vg_maxf(p1
,p2
)), vg_minf(p0
,vg_minf(p1
,p2
)));
557 if( e
> r
) return 0;
562 * Seperating axis test box vs triangle
564 VG_STATIC
int rb_box_triangle_sat( rigidbody
*rba
, v3f tri_src
[3] )
569 v3_sub( rba
->bbx
[1], rba
->bbx
[0], extent
);
570 v3_muls( extent
, 0.5f
, extent
);
571 v3_add( rba
->bbx
[0], extent
, c
);
573 for( int i
=0; i
<3; i
++ )
575 m4x3_mulv( rba
->to_local
, tri_src
[i
], tri
[i
] );
576 v3_sub( tri
[i
], c
, tri
[i
] );
580 if(!rb_box_triangle_interval( extent
, (v3f
){1.0f
,0.0f
,0.0f
}, tri
)) return 0;
581 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,1.0f
,0.0f
}, tri
)) return 0;
582 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,0.0f
,1.0f
}, tri
)) return 0;
584 v3f v0
,v1
,v2
,n
, e0
,e1
,e2
;
585 v3_sub( tri
[1], tri
[0], v0
);
586 v3_sub( tri
[2], tri
[0], v1
);
587 v3_sub( tri
[2], tri
[1], v2
);
591 v3_cross( v0
, v1
, n
);
592 v3_cross( v0
, n
, e0
);
593 v3_cross( n
, v1
, e1
);
594 v3_cross( v2
, n
, e2
);
597 if(!rb_box_triangle_interval( extent
, n
, tri
)) return 0;
600 v3_cross( e0
, (v3f
){1.0f
,0.0f
,0.0f
}, axis
[0] );
601 v3_cross( e0
, (v3f
){0.0f
,1.0f
,0.0f
}, axis
[1] );
602 v3_cross( e0
, (v3f
){0.0f
,0.0f
,1.0f
}, axis
[2] );
603 v3_cross( e1
, (v3f
){1.0f
,0.0f
,0.0f
}, axis
[3] );
604 v3_cross( e1
, (v3f
){0.0f
,1.0f
,0.0f
}, axis
[4] );
605 v3_cross( e1
, (v3f
){0.0f
,0.0f
,1.0f
}, axis
[5] );
606 v3_cross( e2
, (v3f
){1.0f
,0.0f
,0.0f
}, axis
[6] );
607 v3_cross( e2
, (v3f
){0.0f
,1.0f
,0.0f
}, axis
[7] );
608 v3_cross( e2
, (v3f
){0.0f
,0.0f
,1.0f
}, axis
[8] );
610 for( int i
=0; i
<9; i
++ )
611 if(!rb_box_triangle_interval( extent
, axis
[i
], tri
)) return 0;
617 * -----------------------------------------------------------------------------
619 * -----------------------------------------------------------------------------
622 VG_STATIC
int rb_manifold_apply_filtered( rb_ct
*man
, int len
)
626 for( int i
=0; i
<len
; i
++ )
630 if( ct
->type
== k_contact_type_disabled
)
640 * Merge two contacts if they are within radius(r) of eachother
642 VG_STATIC
void rb_manifold_contact_weld( rb_ct
*ci
, rb_ct
*cj
, float r
)
644 if( v3_dist2( ci
->co
, cj
->co
) < r
*r
)
646 cj
->type
= k_contact_type_disabled
;
647 ci
->p
= (ci
->p
+ cj
->p
) * 0.5f
;
649 v3_add( ci
->co
, cj
->co
, ci
->co
);
650 v3_muls( ci
->co
, 0.5f
, ci
->co
);
653 v3_sub( ci
->rba
->co
, ci
->co
, delta
);
655 float c0
= v3_dot( ci
->n
, delta
),
656 c1
= v3_dot( cj
->n
, delta
);
658 if( c0
< 0.0f
|| c1
< 0.0f
)
661 ci
->type
= k_contact_type_disabled
;
666 v3_muls( ci
->n
, c0
, n
);
667 v3_muladds( n
, cj
->n
, c1
, n
);
677 VG_STATIC
void rb_manifold_filter_joint_edges( rb_ct
*man
, int len
, float r
)
679 for( int i
=0; i
<len
-1; i
++ )
682 if( ci
->type
!= k_contact_type_edge
)
685 for( int j
=i
+1; j
<len
; j
++ )
688 if( cj
->type
!= k_contact_type_edge
)
691 rb_manifold_contact_weld( ci
, cj
, r
);
697 * Resolve overlapping pairs
701 VG_STATIC
void rb_manifold_filter_pairs( rb_ct
*man
, int len
, float r
)
703 for( int i
=0; i
<len
-1; i
++ )
708 if( ci
->type
== k_contact_type_disabled
) continue;
710 for( int j
=i
+1; j
<len
; j
++ )
714 if( cj
->type
== k_contact_type_disabled
) continue;
716 if( v3_dist2( ci
->co
, cj
->co
) < r
*r
)
718 cj
->type
= k_contact_type_disabled
;
719 v3_add( cj
->n
, ci
->n
, ci
->n
);
727 float n
= 1.0f
/((float)similar
+1.0f
);
728 v3_muls( ci
->n
, n
, ci
->n
);
731 if( v3_length2(ci
->n
) < 0.1f
*0.1f
)
732 ci
->type
= k_contact_type_disabled
;
734 v3_normalize( ci
->n
);
740 * Remove contacts that are facing away from A
742 VG_STATIC
void rb_manifold_filter_backface( rb_ct
*man
, int len
)
744 for( int i
=0; i
<len
; i
++ )
747 if( ct
->type
== k_contact_type_disabled
)
751 v3_sub( ct
->co
, ct
->rba
->co
, delta
);
753 if( v3_dot( delta
, ct
->n
) > -0.001f
)
754 ct
->type
= k_contact_type_disabled
;
759 * Filter out duplicate coplanar results. Good for spheres.
761 VG_STATIC
void rb_manifold_filter_coplanar( rb_ct
*man
, int len
, float w
)
763 for( int i
=0; i
<len
; i
++ )
766 if( ci
->type
== k_contact_type_disabled
||
767 ci
->type
== k_contact_type_edge
)
770 float d1
= v3_dot( ci
->co
, ci
->n
);
772 for( int j
=0; j
<len
; j
++ )
778 if( cj
->type
== k_contact_type_disabled
)
781 float d2
= v3_dot( cj
->co
, ci
->n
),
784 if( fabsf( d
) <= w
)
786 cj
->type
= k_contact_type_disabled
;
793 * -----------------------------------------------------------------------------
795 * -----------------------------------------------------------------------------
801 * These do not automatically allocate contacts, an appropriately sized
802 * buffer must be supplied. The function returns the size of the manifold
803 * which was generated.
805 * The values set on the contacts are: n, co, p, rba, rbb
809 * By collecting the minimum(time) and maximum(time) pairs of points, we
810 * build a reduced and stable exact manifold.
813 * rx: minimum distance of these points
814 * dx: the delta between the two points
816 * pairs will only ammend these if they are creating a collision
818 typedef struct capsule_manifold capsule_manifold
;
819 struct capsule_manifold
827 * Expand a line manifold with a new pair. t value is the time along segment
828 * on the oriented object which created this pair.
830 VG_STATIC
void rb_capsule_manifold( v3f pa
, v3f pb
, float t
, float r
,
831 capsule_manifold
*manifold
)
834 v3_sub( pa
, pb
, delta
);
836 if( v3_length2(delta
) < r
*r
)
838 if( t
< manifold
->t0
)
840 v3_copy( delta
, manifold
->d0
);
845 if( t
> manifold
->t1
)
847 v3_copy( delta
, manifold
->d1
);
854 VG_STATIC
void rb_capsule_manifold_init( capsule_manifold
*manifold
)
856 manifold
->t0
= INFINITY
;
857 manifold
->t1
= -INFINITY
;
860 VG_STATIC
int rb_capsule_manifold_done( rigidbody
*rba
, rigidbody
*rbb
,
861 capsule_manifold
*manifold
, rb_ct
*buf
)
863 float h
= rba
->inf
.capsule
.height
,
864 ra
= rba
->inf
.capsule
.radius
;
867 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+ra
, p0
);
868 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-ra
, p1
);
871 if( manifold
->t0
<= 1.0f
)
876 v3_muls( p0
, 1.0f
-manifold
->t0
, pa
);
877 v3_muladds( pa
, p1
, manifold
->t0
, pa
);
879 float d
= v3_length( manifold
->d0
);
880 v3_muls( manifold
->d0
, 1.0f
/d
, ct
->n
);
881 v3_muladds( pa
, ct
->n
, -ra
, ct
->co
);
883 ct
->p
= manifold
->r0
- d
;
886 ct
->type
= k_contact_type_default
;
891 if( (manifold
->t1
>= 0.0f
) && (manifold
->t0
!= manifold
->t1
) )
893 rb_ct
*ct
= buf
+count
;
896 v3_muls( p0
, 1.0f
-manifold
->t1
, pa
);
897 v3_muladds( pa
, p1
, manifold
->t1
, pa
);
899 float d
= v3_length( manifold
->d1
);
900 v3_muls( manifold
->d1
, 1.0f
/d
, ct
->n
);
901 v3_muladds( pa
, ct
->n
, -ra
, ct
->co
);
903 ct
->p
= manifold
->r1
- d
;
906 ct
->type
= k_contact_type_default
;
916 vg_line( buf
[0].co
, buf
[1].co
, 0xff0000ff );
921 VG_STATIC
int rb_capsule_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
923 float h
= rba
->inf
.capsule
.height
,
924 ra
= rba
->inf
.capsule
.radius
,
925 rb
= rbb
->inf
.sphere
.radius
;
928 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+ra
, p0
);
929 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-ra
, p1
);
932 closest_point_segment( p0
, p1
, rbb
->co
, c
);
933 v3_sub( c
, rbb
->co
, delta
);
935 float d2
= v3_length2(delta
),
943 v3_muls( delta
, 1.0f
/d
, ct
->n
);
947 v3_muladds( c
, ct
->n
, -ra
, p0
);
948 v3_muladds( rbb
->co
, ct
->n
, rb
, p1
);
949 v3_add( p0
, p1
, ct
->co
);
950 v3_muls( ct
->co
, 0.5f
, ct
->co
);
954 ct
->type
= k_contact_type_default
;
962 VG_STATIC
int rb_capsule_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
964 if( !box_overlap( rba
->bbx_world
, rbb
->bbx_world
) )
967 float ha
= rba
->inf
.capsule
.height
,
968 hb
= rbb
->inf
.capsule
.height
,
969 ra
= rba
->inf
.capsule
.radius
,
970 rb
= rbb
->inf
.capsule
.radius
,
974 v3_muladds( rba
->co
, rba
->up
, -ha
*0.5f
+ra
, p0
);
975 v3_muladds( rba
->co
, rba
->up
, ha
*0.5f
-ra
, p1
);
976 v3_muladds( rbb
->co
, rbb
->up
, -hb
*0.5f
+rb
, p2
);
977 v3_muladds( rbb
->co
, rbb
->up
, hb
*0.5f
-rb
, p3
);
979 capsule_manifold manifold
;
980 rb_capsule_manifold_init( &manifold
);
984 closest_segment_segment( p0
, p1
, p2
, p3
, &ta
, &tb
, pa
, pb
);
985 rb_capsule_manifold( pa
, pb
, ta
, r
, &manifold
);
987 ta
= closest_point_segment( p0
, p1
, p2
, pa
);
988 tb
= closest_point_segment( p0
, p1
, p3
, pb
);
989 rb_capsule_manifold( pa
, p2
, ta
, r
, &manifold
);
990 rb_capsule_manifold( pb
, p3
, tb
, r
, &manifold
);
992 closest_point_segment( p2
, p3
, p0
, pa
);
993 closest_point_segment( p2
, p3
, p1
, pb
);
994 rb_capsule_manifold( p0
, pa
, 0.0f
, r
, &manifold
);
995 rb_capsule_manifold( p1
, pb
, 1.0f
, r
, &manifold
);
997 return rb_capsule_manifold_done( rba
, rbb
, &manifold
, buf
);
1001 * Generates up to two contacts; optimised for the most stable manifold
1003 VG_STATIC
int rb_capsule_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1005 float h
= rba
->inf
.capsule
.height
,
1006 r
= rba
->inf
.capsule
.radius
;
1009 * Solving this in symetric local space of the cube saves us some time and a
1010 * couple branches when it comes to the quad stage.
1013 v3_add( rbb
->bbx
[0], rbb
->bbx
[1], centroid
);
1014 v3_muls( centroid
, 0.5f
, centroid
);
1017 v3_sub( rbb
->bbx
[0], centroid
, bbx
[0] );
1018 v3_sub( rbb
->bbx
[1], centroid
, bbx
[1] );
1020 v3f pc
, p0w
, p1w
, p0
, p1
;
1021 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+r
, p0w
);
1022 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-r
, p1w
);
1024 m4x3_mulv( rbb
->to_local
, p0w
, p0
);
1025 m4x3_mulv( rbb
->to_local
, p1w
, p1
);
1026 v3_sub( p0
, centroid
, p0
);
1027 v3_sub( p1
, centroid
, p1
);
1028 v3_add( p0
, p1
, pc
);
1029 v3_muls( pc
, 0.5f
, pc
);
1032 * Finding an appropriate quad to collide lines with
1035 v3_div( pc
, bbx
[1], region
);
1038 if( (fabsf(region
[0]) > fabsf(region
[1])) &&
1039 (fabsf(region
[0]) > fabsf(region
[2])) )
1041 float px
= vg_signf(region
[0]) * bbx
[1][0];
1042 v3_copy( (v3f
){ px
, bbx
[0][1], bbx
[0][2] }, quad
[0] );
1043 v3_copy( (v3f
){ px
, bbx
[1][1], bbx
[0][2] }, quad
[1] );
1044 v3_copy( (v3f
){ px
, bbx
[1][1], bbx
[1][2] }, quad
[2] );
1045 v3_copy( (v3f
){ px
, bbx
[0][1], bbx
[1][2] }, quad
[3] );
1047 else if( fabsf(region
[1]) > fabsf(region
[2]) )
1049 float py
= vg_signf(region
[1]) * bbx
[1][1];
1050 v3_copy( (v3f
){ bbx
[0][0], py
, bbx
[0][2] }, quad
[0] );
1051 v3_copy( (v3f
){ bbx
[1][0], py
, bbx
[0][2] }, quad
[1] );
1052 v3_copy( (v3f
){ bbx
[1][0], py
, bbx
[1][2] }, quad
[2] );
1053 v3_copy( (v3f
){ bbx
[0][0], py
, bbx
[1][2] }, quad
[3] );
1057 float pz
= vg_signf(region
[2]) * bbx
[1][2];
1058 v3_copy( (v3f
){ bbx
[0][0], bbx
[0][1], pz
}, quad
[0] );
1059 v3_copy( (v3f
){ bbx
[1][0], bbx
[0][1], pz
}, quad
[1] );
1060 v3_copy( (v3f
){ bbx
[1][0], bbx
[1][1], pz
}, quad
[2] );
1061 v3_copy( (v3f
){ bbx
[0][0], bbx
[1][1], pz
}, quad
[3] );
1064 capsule_manifold manifold
;
1065 rb_capsule_manifold_init( &manifold
);
1068 closest_point_aabb( p0
, bbx
, c0
);
1069 closest_point_aabb( p1
, bbx
, c1
);
1072 v3_sub( c0
, p0
, d0
);
1073 v3_sub( c1
, p1
, d1
);
1074 v3_sub( p1
, p0
, da
);
1080 if( v3_dot( da
, d0
) <= 0.01f
)
1081 rb_capsule_manifold( p0
, c0
, 0.0f
, r
, &manifold
);
1083 if( v3_dot( da
, d1
) >= -0.01f
)
1084 rb_capsule_manifold( p1
, c1
, 1.0f
, r
, &manifold
);
1086 for( int i
=0; i
<4; i
++ )
1093 closest_segment_segment( p0
, p1
, quad
[i0
], quad
[i1
], &ta
, &tb
, ca
, cb
);
1094 rb_capsule_manifold( ca
, cb
, ta
, r
, &manifold
);
1098 * Create final contacts based on line manifold
1100 m3x3_mulv( rbb
->to_world
, manifold
.d0
, manifold
.d0
);
1101 m3x3_mulv( rbb
->to_world
, manifold
.d1
, manifold
.d1
);
1108 for( int i
=0; i
<4; i
++ )
1114 v3_add( quad
[i0
], centroid
, q0
);
1115 v3_add( quad
[i1
], centroid
, q1
);
1117 m4x3_mulv( rbb
->to_world
, q0
, q0
);
1118 m4x3_mulv( rbb
->to_world
, q1
, q1
);
1120 vg_line( q0
, q1
, 0xffffffff );
1124 return rb_capsule_manifold_done( rba
, rbb
, &manifold
, buf
);
1127 VG_STATIC
int rb_sphere_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1131 closest_point_obb( rba
->co
, rbb
->bbx
, rbb
->to_world
, rbb
->to_local
, co
);
1132 v3_sub( rba
->co
, co
, delta
);
1134 float d2
= v3_length2(delta
),
1135 r
= rba
->inf
.sphere
.radius
;
1144 v3_sub( rba
->co
, rbb
->co
, delta
);
1147 * some extra testing is required to find the best axis to push the
1148 * object back outside the box. Since there isnt a clear seperating
1149 * vector already, especially on really high aspect boxes.
1151 float lx
= v3_dot( rbb
->right
, delta
),
1152 ly
= v3_dot( rbb
->up
, delta
),
1153 lz
= v3_dot( rbb
->forward
, delta
),
1154 px
= rbb
->bbx
[1][0] - fabsf(lx
),
1155 py
= rbb
->bbx
[1][1] - fabsf(ly
),
1156 pz
= rbb
->bbx
[1][2] - fabsf(lz
);
1158 if( px
< py
&& px
< pz
)
1159 v3_muls( rbb
->right
, vg_signf(lx
), ct
->n
);
1161 v3_muls( rbb
->up
, vg_signf(ly
), ct
->n
);
1163 v3_muls( rbb
->forward
, vg_signf(lz
), ct
->n
);
1165 v3_muladds( rba
->co
, ct
->n
, -r
, ct
->co
);
1171 v3_muls( delta
, 1.0f
/d
, ct
->n
);
1173 v3_copy( co
, ct
->co
);
1178 ct
->type
= k_contact_type_default
;
1185 VG_STATIC
int rb_sphere_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1188 v3_sub( rba
->co
, rbb
->co
, delta
);
1190 float d2
= v3_length2(delta
),
1191 r
= rba
->inf
.sphere
.radius
+ rbb
->inf
.sphere
.radius
;
1195 float d
= sqrtf(d2
);
1198 v3_muls( delta
, 1.0f
/d
, ct
->n
);
1201 v3_muladds( rba
->co
, ct
->n
,-rba
->inf
.sphere
.radius
, p0
);
1202 v3_muladds( rbb
->co
, ct
->n
, rbb
->inf
.sphere
.radius
, p1
);
1203 v3_add( p0
, p1
, ct
->co
);
1204 v3_muls( ct
->co
, 0.5f
, ct
->co
);
1205 ct
->type
= k_contact_type_default
;
1215 //#define RIGIDBODY_DYNAMIC_MESH_EDGES
1217 VG_STATIC
int rb_sphere_triangle( rigidbody
*rba
, rigidbody
*rbb
,
1218 v3f tri
[3], rb_ct
*buf
)
1222 #ifdef RIGIDBODY_DYNAMIC_MESH_EDGES
1223 closest_on_triangle_1( rba
->co
, tri
, co
);
1225 enum contact_type type
= closest_on_triangle_1( rba
->co
, tri
, co
);
1228 v3_sub( rba
->co
, co
, delta
);
1230 float d2
= v3_length2( delta
),
1231 r
= rba
->inf
.sphere
.radius
;
1238 v3_sub( tri
[2], tri
[0], ab
);
1239 v3_sub( tri
[1], tri
[0], ac
);
1240 v3_cross( ac
, ab
, tn
);
1241 v3_copy( tn
, ct
->n
);
1243 if( v3_length2( ct
->n
) <= 0.00001f
)
1245 vg_error( "Zero area triangle!\n" );
1249 v3_normalize( ct
->n
);
1251 float d
= sqrtf(d2
);
1253 v3_copy( co
, ct
->co
);
1265 VG_STATIC
void rb_debug_sharp_scene_edges( rigidbody
*rbb
, float sharp_ang
,
1266 boxf box
, u32 colour
)
1268 sharp_ang
= cosf( sharp_ang
);
1270 scene
*sc
= rbb
->inf
.scene
.bh_scene
->user
;
1271 vg_line_boxf( box
, 0xff00ff00 );
1274 bh_iter_init( 0, &it
);
1277 while( bh_next( rbb
->inf
.scene
.bh_scene
, &it
, box
, &idx
) )
1279 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
1282 for( int j
=0; j
<3; j
++ )
1283 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
1285 for( int j
=0; j
<3; j
++ )
1289 v3_sub( tri
[(j
+1)%3], tri
[j
], edir
);
1291 if( v3_dot( edir
, (v3f
){ 0.5184758473652127f
,
1292 0.2073903389460850f
,
1293 -0.8295613557843402f
} ) < 0.0f
)
1298 bh_iter_init( 0, &jt
);
1301 float const k_r
= 0.02f
;
1302 v3_add( (v3f
){ k_r
, k_r
, k_r
}, tri
[j
], region
[1] );
1303 v3_add( (v3f
){ -k_r
, -k_r
, -k_r
}, tri
[j
], region
[0] );
1306 while( bh_next( rbb
->inf
.scene
.bh_scene
, &jt
, region
, &jdx
) )
1311 u32
*ptrj
= &sc
->arrindices
[ jdx
*3 ];
1314 for( int k
=0; k
<3; k
++ )
1315 v3_copy( sc
->arrvertices
[ptrj
[k
]].co
, trj
[k
] );
1317 for( int k
=0; k
<3; k
++ )
1319 if( v3_dist2( tri
[j
], trj
[k
] ) <= k_r
*k_r
)
1326 if( v3_dist2( tri
[jp1
], trj
[km1
] ) <= k_r
*k_r
)
1329 v3_sub( tri
[jp1
], tri
[j
], b0
);
1330 v3_sub( tri
[jp2
], tri
[j
], b1
);
1331 v3_sub( trj
[km2
], tri
[j
], b2
);
1334 v3_cross( b0
, b1
, cx0
);
1335 v3_cross( b2
, b0
, cx1
);
1337 float polarity
= v3_dot( cx0
, b2
);
1339 if( polarity
< 0.0f
)
1342 vg_line( tri
[j
], tri
[jp1
], 0xff00ff00 );
1343 float ang
= v3_dot(cx0
,cx1
) /
1344 (v3_length(cx0
)*v3_length(cx1
));
1345 if( ang
< sharp_ang
)
1347 vg_line( tri
[j
], tri
[jp1
], 0xff00ff00 );
1359 VG_STATIC
int rb_sphere_scene( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1361 scene
*sc
= rbb
->inf
.scene
.bh_scene
->user
;
1364 bh_iter_init( 0, &it
);
1369 while( bh_next( rbb
->inf
.scene
.bh_scene
, &it
, rba
->bbx_world
, &idx
) )
1371 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
1374 for( int j
=0; j
<3; j
++ )
1375 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
1377 buf
[ count
].element_id
= ptri
[0];
1379 vg_line( tri
[0],tri
[1],0x70ff6000 );
1380 vg_line( tri
[1],tri
[2],0x70ff6000 );
1381 vg_line( tri
[2],tri
[0],0x70ff6000 );
1383 int contact
= rb_sphere_triangle( rba
, rbb
, tri
, buf
+count
);
1388 vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
1396 VG_STATIC
int rb_box_scene( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1398 scene
*sc
= rbb
->inf
.scene
.bh_scene
->user
;
1403 bh_iter_init( 0, &it
);
1408 while( bh_next( rbb
->inf
.scene
.bh_scene
, &it
, rba
->bbx_world
, &idx
) )
1410 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
1412 for( int j
=0; j
<3; j
++ )
1413 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
1415 if( rb_box_triangle_sat( rba
, tri
) )
1417 vg_line(tri
[0],tri
[1],0xff50ff00 );
1418 vg_line(tri
[1],tri
[2],0xff50ff00 );
1419 vg_line(tri
[2],tri
[0],0xff50ff00 );
1423 vg_line(tri
[0],tri
[1],0xff0000ff );
1424 vg_line(tri
[1],tri
[2],0xff0000ff );
1425 vg_line(tri
[2],tri
[0],0xff0000ff );
1431 v3_sub( tri
[1], tri
[0], v0
);
1432 v3_sub( tri
[2], tri
[0], v1
);
1433 v3_cross( v0
, v1
, n
);
1436 /* find best feature */
1437 float best
= v3_dot( rba
->right
, n
);
1440 float cy
= v3_dot( rba
->up
, n
);
1441 if( fabsf(cy
) > fabsf(best
) )
1447 float cz
= -v3_dot( rba
->forward
, n
);
1448 if( fabsf(cz
) > fabsf(best
) )
1458 float px
= best
> 0.0f
? rba
->bbx
[0][0]: rba
->bbx
[1][0];
1459 manifold
[0][0] = px
;
1460 manifold
[0][1] = rba
->bbx
[0][1];
1461 manifold
[0][2] = rba
->bbx
[0][2];
1462 manifold
[1][0] = px
;
1463 manifold
[1][1] = rba
->bbx
[1][1];
1464 manifold
[1][2] = rba
->bbx
[0][2];
1465 manifold
[2][0] = px
;
1466 manifold
[2][1] = rba
->bbx
[1][1];
1467 manifold
[2][2] = rba
->bbx
[1][2];
1468 manifold
[3][0] = px
;
1469 manifold
[3][1] = rba
->bbx
[0][1];
1470 manifold
[3][2] = rba
->bbx
[1][2];
1472 else if( axis
== 1 )
1474 float py
= best
> 0.0f
? rba
->bbx
[0][1]: rba
->bbx
[1][1];
1475 manifold
[0][0] = rba
->bbx
[0][0];
1476 manifold
[0][1] = py
;
1477 manifold
[0][2] = rba
->bbx
[0][2];
1478 manifold
[1][0] = rba
->bbx
[1][0];
1479 manifold
[1][1] = py
;
1480 manifold
[1][2] = rba
->bbx
[0][2];
1481 manifold
[2][0] = rba
->bbx
[1][0];
1482 manifold
[2][1] = py
;
1483 manifold
[2][2] = rba
->bbx
[1][2];
1484 manifold
[3][0] = rba
->bbx
[0][0];
1485 manifold
[3][1] = py
;
1486 manifold
[3][2] = rba
->bbx
[1][2];
1490 float pz
= best
> 0.0f
? rba
->bbx
[0][2]: rba
->bbx
[1][2];
1491 manifold
[0][0] = rba
->bbx
[0][0];
1492 manifold
[0][1] = rba
->bbx
[0][1];
1493 manifold
[0][2] = pz
;
1494 manifold
[1][0] = rba
->bbx
[1][0];
1495 manifold
[1][1] = rba
->bbx
[0][1];
1496 manifold
[1][2] = pz
;
1497 manifold
[2][0] = rba
->bbx
[1][0];
1498 manifold
[2][1] = rba
->bbx
[1][1];
1499 manifold
[2][2] = pz
;
1500 manifold
[3][0] = rba
->bbx
[0][0];
1501 manifold
[3][1] = rba
->bbx
[1][1];
1502 manifold
[3][2] = pz
;
1505 for( int j
=0; j
<4; j
++ )
1506 m4x3_mulv( rba
->to_world
, manifold
[j
], manifold
[j
] );
1508 vg_line( manifold
[0], manifold
[1], 0xffffffff );
1509 vg_line( manifold
[1], manifold
[2], 0xffffffff );
1510 vg_line( manifold
[2], manifold
[3], 0xffffffff );
1511 vg_line( manifold
[3], manifold
[0], 0xffffffff );
1513 for( int j
=0; j
<4; j
++ )
1515 rb_ct
*ct
= buf
+count
;
1517 v3_copy( manifold
[j
], ct
->co
);
1518 v3_copy( n
, ct
->n
);
1520 float l0
= v3_dot( tri
[0], n
),
1521 l1
= v3_dot( manifold
[j
], n
);
1523 ct
->p
= (l0
-l1
)*0.5f
;
1527 ct
->type
= k_contact_type_default
;
1540 * Generates up to two contacts; optimised for the most stable manifold
1542 VG_STATIC
int rb_capsule_triangle( rigidbody
*rba
, rigidbody
*rbb
,
1543 v3f tri
[3], rb_ct
*buf
)
1545 float h
= rba
->inf
.capsule
.height
,
1546 r
= rba
->inf
.capsule
.radius
;
1549 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+r
, p0w
);
1550 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-r
, p1w
);
1552 capsule_manifold manifold
;
1553 rb_capsule_manifold_init( &manifold
);
1556 closest_on_triangle_1( p0w
, tri
, c0
);
1557 closest_on_triangle_1( p1w
, tri
, c1
);
1560 v3_sub( c0
, p0w
, d0
);
1561 v3_sub( c1
, p1w
, d1
);
1562 v3_sub( p1w
, p0w
, da
);
1568 if( v3_dot( da
, d0
) <= 0.01f
)
1569 rb_capsule_manifold( p0w
, c0
, 0.0f
, r
, &manifold
);
1571 if( v3_dot( da
, d1
) >= -0.01f
)
1572 rb_capsule_manifold( p1w
, c1
, 1.0f
, r
, &manifold
);
1574 for( int i
=0; i
<3; i
++ )
1581 closest_segment_segment( p0w
, p1w
, tri
[i0
], tri
[i1
], &ta
, &tb
, ca
, cb
);
1582 rb_capsule_manifold( ca
, cb
, ta
, r
, &manifold
);
1586 v3_sub( tri
[1], tri
[0], v0
);
1587 v3_sub( tri
[2], tri
[0], v1
);
1588 v3_cross( v0
, v1
, n
);
1591 int count
= rb_capsule_manifold_done( rba
, rbb
, &manifold
, buf
);
1592 for( int i
=0; i
<count
; i
++ )
1593 v3_copy( n
, buf
[i
].n
);
1598 VG_STATIC
int rb_capsule_scene( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1601 float h
= rba
->inf
.capsule
.height
,
1602 r
= rba
->inf
.capsule
.radius
,
1606 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+r
, p
[0] );
1607 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-r
, p
[1] );
1612 for( int i
=0; i
<2; i
++ )
1614 if( p
[i
][1] < g
+ r
)
1616 rb_ct
*ct
= &buf
[ count
++ ];
1618 v3_copy( p
[i
], ct
->co
);
1619 ct
->p
= r
- (p
[i
][1]-g
);
1621 v3_copy( (v3f
){0.0f
,1.0f
,0.0f
}, ct
->n
);
1624 ct
->type
= k_contact_type_default
;
1631 scene
*sc
= rbb
->inf
.scene
.bh_scene
->user
;
1634 bh_iter_init( 0, &it
);
1639 while( bh_next( rbb
->inf
.scene
.bh_scene
, &it
, rba
->bbx_world
, &idx
) )
1641 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
1644 for( int j
=0; j
<3; j
++ )
1645 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
1647 buf
[ count
].element_id
= ptri
[0];
1650 vg_line( tri
[0],tri
[1],0x70ff6000 );
1651 vg_line( tri
[1],tri
[2],0x70ff6000 );
1652 vg_line( tri
[2],tri
[0],0x70ff6000 );
1655 int contact
= rb_capsule_triangle( rba
, rbb
, tri
, buf
+count
);
1660 vg_warn("Exceeding capsule_vs_scene capacity. Geometry too dense!\n");
1669 VG_STATIC
int rb_scene_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1671 return rb_capsule_scene( rbb
, rba
, buf
);
1674 VG_STATIC
int RB_MATRIX_ERROR( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1677 vg_error( "Collision type is unimplemented between types %d and %d\n",
1678 rba
->type
, rbb
->type
);
1684 VG_STATIC
int rb_sphere_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1686 return rb_capsule_sphere( rbb
, rba
, buf
);
1689 VG_STATIC
int rb_box_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1691 return rb_capsule_box( rbb
, rba
, buf
);
1694 VG_STATIC
int rb_box_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1696 return rb_sphere_box( rbb
, rba
, buf
);
1699 VG_STATIC
int rb_scene_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1701 return rb_box_scene( rbb
, rba
, buf
);
1704 VG_STATIC
int (*rb_jump_table
[4][4])( rigidbody
*a
, rigidbody
*b
, rb_ct
*buf
) =
1706 /* box */ /* Sphere */ /* Capsule */ /* Mesh */
1707 { RB_MATRIX_ERROR
, rb_box_sphere
, rb_box_capsule
, rb_box_scene
},
1708 { rb_sphere_box
, rb_sphere_sphere
, rb_sphere_capsule
, rb_sphere_scene
},
1709 { rb_capsule_box
, rb_capsule_sphere
, rb_capsule_capsule
, rb_capsule_scene
},
1710 { rb_scene_box
, RB_MATRIX_ERROR
, rb_scene_capsule
, RB_MATRIX_ERROR
}
1713 VG_STATIC
int rb_collide( rigidbody
*rba
, rigidbody
*rbb
)
1715 int (*collider_jump
)(rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1716 = rb_jump_table
[rba
->type
][rbb
->type
];
1719 * 12 is the maximum manifold size we can generate, so we are forced to abort
1720 * potentially checking any more.
1722 if( rb_contact_count
+ 12 > vg_list_size(rb_contact_buffer
) )
1724 vg_warn( "Too many contacts made in global collider buffer (%d of %d\n)",
1725 rb_contact_count
, vg_list_size(rb_contact_buffer
) );
1730 * FUTURE: Replace this with a more dedicated broad phase pass
1732 if( box_overlap( rba
->bbx_world
, rbb
->bbx_world
) )
1734 int count
= collider_jump( rba
, rbb
, rb_contact_buffer
+rb_contact_count
);
1735 rb_contact_count
+= count
;
1743 * -----------------------------------------------------------------------------
1745 * -----------------------------------------------------------------------------
1748 VG_STATIC
void rb_solver_reset(void)
1750 rb_contact_count
= 0;
1753 VG_STATIC rb_ct
*rb_global_ct(void)
1755 return rb_contact_buffer
+ rb_contact_count
;
1759 * Initializing things like tangent vectors
1761 VG_STATIC
void rb_presolve_contacts( rb_ct
*buffer
, int len
)
1763 for( int i
=0; i
<len
; i
++ )
1765 rb_ct
*ct
= &buffer
[i
];
1767 ct
->bias
= -0.2f
* k_rb_rate
* vg_minf( 0.0f
, -ct
->p
+k_penetration_slop
);
1768 rb_tangent_basis( ct
->n
, ct
->t
[0], ct
->t
[1] );
1771 ct
->type
= k_contact_type_default
;
1773 ct
->norm_impulse
= 0.0f
;
1774 ct
->tangent_impulse
[0] = 0.0f
;
1775 ct
->tangent_impulse
[1] = 0.0f
;
1777 v3f ra
, rb
, raCn
, rbCn
, raCt
, rbCt
;
1778 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
1779 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
1780 v3_cross( ra
, ct
->n
, raCn
);
1781 v3_cross( rb
, ct
->n
, rbCn
);
1783 /* orient inverse inertia tensors */
1785 m3x3_mulv( ct
->rba
->iIw
, raCn
, raCnI
);
1786 m3x3_mulv( ct
->rbb
->iIw
, rbCn
, rbCnI
);
1788 ct
->normal_mass
= ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
1789 ct
->normal_mass
+= v3_dot( raCn
, raCnI
);
1790 ct
->normal_mass
+= v3_dot( rbCn
, rbCnI
);
1791 ct
->normal_mass
= 1.0f
/ct
->normal_mass
;
1793 for( int j
=0; j
<2; j
++ )
1796 v3_cross( ct
->t
[j
], ra
, raCt
);
1797 v3_cross( ct
->t
[j
], rb
, rbCt
);
1798 m3x3_mulv( ct
->rba
->iIw
, raCt
, raCtI
);
1799 m3x3_mulv( ct
->rbb
->iIw
, rbCt
, rbCtI
);
1801 ct
->tangent_mass
[j
] = ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
1802 ct
->tangent_mass
[j
] += v3_dot( raCt
, raCtI
);
1803 ct
->tangent_mass
[j
] += v3_dot( rbCt
, rbCtI
);
1804 ct
->tangent_mass
[j
] = 1.0f
/ct
->tangent_mass
[j
];
1807 rb_debug_contact( ct
);
1812 * Creates relative contact velocity vector
1814 VG_STATIC
void rb_rcv( rigidbody
*rba
, rigidbody
*rbb
, v3f ra
, v3f rb
, v3f rv
)
1817 v3_cross( rba
->w
, ra
, rva
);
1818 v3_add( rba
->v
, rva
, rva
);
1819 v3_cross( rbb
->w
, rb
, rvb
);
1820 v3_add( rbb
->v
, rvb
, rvb
);
1822 v3_sub( rva
, rvb
, rv
);
1826 * Apply impulse to object
1828 VG_STATIC
void rb_linear_impulse( rigidbody
*rb
, v3f delta
, v3f impulse
)
1831 v3_muladds( rb
->v
, impulse
, rb
->inv_mass
, rb
->v
);
1833 /* Angular velocity */
1835 v3_cross( delta
, impulse
, wa
);
1837 m3x3_mulv( rb
->iIw
, wa
, wa
);
1838 v3_add( rb
->w
, wa
, rb
->w
);
1842 * One iteration to solve the contact constraint
1844 VG_STATIC
void rb_solve_contacts( rb_ct
*buf
, int len
)
1846 for( int i
=0; i
<len
; i
++ )
1848 struct contact
*ct
= &buf
[i
];
1851 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
1852 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
1853 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
1856 for( int j
=0; j
<2; j
++ )
1858 float f
= k_friction
* ct
->norm_impulse
,
1859 vt
= v3_dot( rv
, ct
->t
[j
] ),
1860 lambda
= ct
->tangent_mass
[j
] * -vt
;
1862 float temp
= ct
->tangent_impulse
[j
];
1863 ct
->tangent_impulse
[j
] = vg_clampf( temp
+ lambda
, -f
, f
);
1864 lambda
= ct
->tangent_impulse
[j
] - temp
;
1867 v3_muls( ct
->t
[j
], lambda
, impulse
);
1868 rb_linear_impulse( ct
->rba
, ra
, impulse
);
1870 v3_muls( ct
->t
[j
], -lambda
, impulse
);
1871 rb_linear_impulse( ct
->rbb
, rb
, impulse
);
1875 rb_rcv( ct
->rba
, ct
->rbb
, ra
, rb
, rv
);
1876 float vn
= v3_dot( rv
, ct
->n
),
1877 lambda
= ct
->normal_mass
* (-vn
+ ct
->bias
);
1879 float temp
= ct
->norm_impulse
;
1880 ct
->norm_impulse
= vg_maxf( temp
+ lambda
, 0.0f
);
1881 lambda
= ct
->norm_impulse
- temp
;
1884 v3_muls( ct
->n
, lambda
, impulse
);
1885 rb_linear_impulse( ct
->rba
, ra
, impulse
);
1887 v3_muls( ct
->n
, -lambda
, impulse
);
1888 rb_linear_impulse( ct
->rbb
, rb
, impulse
);
1893 * -----------------------------------------------------------------------------
1895 * -----------------------------------------------------------------------------
1898 VG_STATIC
void rb_debug_position_constraints( rb_constr_pos
*buffer
, int len
)
1900 for( int i
=0; i
<len
; i
++ )
1902 rb_constr_pos
*constr
= &buffer
[i
];
1903 rigidbody
*rba
= constr
->rba
, *rbb
= constr
->rbb
;
1906 m3x3_mulv( rba
->to_world
, constr
->lca
, wca
);
1907 m3x3_mulv( rbb
->to_world
, constr
->lcb
, wcb
);
1910 v3_add( wca
, rba
->co
, p0
);
1911 v3_add( wcb
, rbb
->co
, p1
);
1912 vg_line_pt3( p0
, 0.0025f
, 0xff000000 );
1913 vg_line_pt3( p1
, 0.0025f
, 0xffffffff );
1914 vg_line2( p0
, p1
, 0xff000000, 0xffffffff );
1918 VG_STATIC
void rb_presolve_swingtwist_constraints( rb_constr_swingtwist
*buf
,
1923 for( int i
=0; i
<len
; i
++ )
1925 rb_constr_swingtwist
*st
= &buf
[ i
];
1927 v3f vx
, vy
, va
, vxb
, axis
, center
;
1929 m3x3_mulv( st
->rba
->to_world
, st
->conevx
, vx
);
1930 m3x3_mulv( st
->rbb
->to_world
, st
->conevxb
, vxb
);
1931 m3x3_mulv( st
->rba
->to_world
, st
->conevy
, vy
);
1932 m3x3_mulv( st
->rbb
->to_world
, st
->coneva
, va
);
1933 m4x3_mulv( st
->rba
->to_world
, st
->view_offset
, center
);
1934 v3_cross( vy
, vx
, axis
);
1936 /* Constraint violated ? */
1937 float fx
= v3_dot( vx
, va
), /* projection world */
1938 fy
= v3_dot( vy
, va
),
1939 fn
= v3_dot( va
, axis
),
1941 rx
= st
->conevx
[3], /* elipse radii */
1944 lx
= fx
/rx
, /* projection local (fn==lz) */
1947 st
->tangent_violation
= ((lx
*lx
+ ly
*ly
) > fn
*fn
) || (fn
<= 0.0f
);
1948 if( st
->tangent_violation
)
1950 /* Calculate a good position and the axis to solve on */
1951 v2f closest
, tangent
,
1952 p
= { fx
/fabsf(fn
), fy
/fabsf(fn
) };
1954 closest_point_elipse( p
, (v2f
){rx
,ry
}, closest
);
1955 tangent
[0] = -closest
[1] / (ry
*ry
);
1956 tangent
[1] = closest
[0] / (rx
*rx
);
1957 v2_normalize( tangent
);
1960 v3_muladds( axis
, vx
, closest
[0], v0
);
1961 v3_muladds( v0
, vy
, closest
[1], v0
);
1964 v3_muls( vx
, tangent
[0], v1
);
1965 v3_muladds( v1
, vy
, tangent
[1], v1
);
1967 v3_copy( v0
, st
->tangent_target
);
1968 v3_copy( v1
, st
->tangent_axis
);
1970 /* calculate mass */
1972 m3x3_mulv( st
->rba
->iIw
, st
->tangent_axis
, aIw
);
1973 m3x3_mulv( st
->rbb
->iIw
, st
->tangent_axis
, bIw
);
1974 st
->tangent_mass
= 1.0f
/ (v3_dot( st
->tangent_axis
, aIw
) +
1975 v3_dot( st
->tangent_axis
, bIw
));
1977 float angle
= v3_dot( va
, st
->tangent_target
);
1981 v3_cross( vy
, va
, refaxis
); /* our default rotation */
1982 v3_normalize( refaxis
);
1984 float angle
= v3_dot( refaxis
, vxb
);
1985 st
->axis_violation
= fabsf(angle
) < st
->conet
;
1987 if( st
->axis_violation
)
1990 v3_cross( refaxis
, vxb
, dir_test
);
1992 if( v3_dot(dir_test
, va
) < 0.0f
)
1993 st
->axis_violation
= -st
->axis_violation
;
1995 float newang
= (float)st
->axis_violation
* acosf(st
->conet
-0.0001f
);
1998 v3_cross( va
, refaxis
, refaxis_up
);
1999 v3_muls( refaxis_up
, sinf(newang
), st
->axis_target
);
2000 v3_muladds( st
->axis_target
, refaxis
, -cosf(newang
), st
->axis_target
);
2002 /* calculate mass */
2003 v3_copy( va
, st
->axis
);
2005 m3x3_mulv( st
->rba
->iIw
, st
->axis
, aIw
);
2006 m3x3_mulv( st
->rbb
->iIw
, st
->axis
, bIw
);
2007 st
->axis_mass
= 1.0f
/ (v3_dot( st
->axis
, aIw
) +
2008 v3_dot( st
->axis
, bIw
));
2013 VG_STATIC
void rb_debug_swingtwist_constraints( rb_constr_swingtwist
*buf
,
2018 for( int i
=0; i
<len
; i
++ )
2020 rb_constr_swingtwist
*st
= &buf
[ i
];
2022 v3f vx
, vxb
, vy
, va
, axis
, center
;
2024 m3x3_mulv( st
->rba
->to_world
, st
->conevx
, vx
);
2025 m3x3_mulv( st
->rbb
->to_world
, st
->conevxb
, vxb
);
2026 m3x3_mulv( st
->rba
->to_world
, st
->conevy
, vy
);
2027 m3x3_mulv( st
->rbb
->to_world
, st
->coneva
, va
);
2028 m4x3_mulv( st
->rba
->to_world
, st
->view_offset
, center
);
2029 v3_cross( vy
, vx
, axis
);
2031 float rx
= st
->conevx
[3], /* elipse radii */
2035 v3_muladds( center
, va
, size
, p1
);
2036 vg_line( center
, p1
, 0xffffffff );
2037 vg_line_pt3( p1
, 0.00025f
, 0xffffffff );
2039 if( st
->tangent_violation
)
2041 v3_muladds( center
, st
->tangent_target
, size
, p0
);
2043 vg_line( center
, p0
, 0xff00ff00 );
2044 vg_line_pt3( p0
, 0.00025f
, 0xff00ff00 );
2045 vg_line( p1
, p0
, 0xff000000 );
2048 for( int x
=0; x
<32; x
++ )
2050 float t0
= ((float)x
* (1.0f
/32.0f
)) * VG_TAUf
,
2051 t1
= (((float)x
+1.0f
) * (1.0f
/32.0f
)) * VG_TAUf
,
2058 v3_muladds( axis
, vx
, c0
*rx
, v0
);
2059 v3_muladds( v0
, vy
, s0
*ry
, v0
);
2060 v3_muladds( axis
, vx
, c1
*rx
, v1
);
2061 v3_muladds( v1
, vy
, s1
*ry
, v1
);
2066 v3_muladds( center
, v0
, size
, p0
);
2067 v3_muladds( center
, v1
, size
, p1
);
2069 u32 col0r
= fabsf(c0
) * 255.0f
,
2070 col0g
= fabsf(s0
) * 255.0f
,
2071 col1r
= fabsf(c1
) * 255.0f
,
2072 col1g
= fabsf(s1
) * 255.0f
,
2073 col
= st
->tangent_violation
? 0xff0000ff: 0xff000000,
2074 col0
= col
| (col0r
<<16) | (col0g
<< 8),
2075 col1
= col
| (col1r
<<16) | (col1g
<< 8);
2077 vg_line2( center
, p0
, VG__NONE
, col0
);
2078 vg_line2( p0
, p1
, col0
, col1
);
2082 v3_muladds( center
, va
, size
, p0
);
2083 v3_muladds( p0
, vxb
, size
, p1
);
2085 vg_line( p0
, p1
, 0xff0000ff );
2087 if( st
->axis_violation
)
2089 v3_muladds( p0
, st
->axis_target
, size
*1.25f
, p1
);
2090 vg_line( p0
, p1
, 0xffffff00 );
2091 vg_line_pt3( p1
, 0.0025f
, 0xffffff80 );
2095 v3_cross( vy
, va
, refaxis
); /* our default rotation */
2096 v3_normalize( refaxis
);
2098 v3_cross( va
, refaxis
, refaxis_up
);
2099 float newang
= acosf(st
->conet
-0.0001f
);
2101 v3_muladds( p0
, refaxis_up
, sinf(newang
)*size
, p1
);
2102 v3_muladds( p1
, refaxis
, -cosf(newang
)*size
, p1
);
2103 vg_line( p0
, p1
, 0xff000000 );
2105 v3_muladds( p0
, refaxis_up
, sinf(-newang
)*size
, p1
);
2106 v3_muladds( p1
, refaxis
, -cosf(-newang
)*size
, p1
);
2107 vg_line( p0
, p1
, 0xff404040 );
2112 * Solve a list of positional constraints
2114 VG_STATIC
void rb_solve_position_constraints( rb_constr_pos
*buf
, int len
)
2116 for( int i
=0; i
<len
; i
++ )
2118 rb_constr_pos
*constr
= &buf
[i
];
2119 rigidbody
*rba
= constr
->rba
, *rbb
= constr
->rbb
;
2122 m3x3_mulv( rba
->to_world
, constr
->lca
, wa
);
2123 m3x3_mulv( rbb
->to_world
, constr
->lcb
, wb
);
2125 m3x3f ssra
, ssrat
, ssrb
, ssrbt
;
2127 m3x3_skew_symetric( ssrat
, wa
);
2128 m3x3_skew_symetric( ssrbt
, wb
);
2129 m3x3_transpose( ssrat
, ssra
);
2130 m3x3_transpose( ssrbt
, ssrb
);
2132 v3f b
, b_wa
, b_wb
, b_a
, b_b
;
2133 m3x3_mulv( ssra
, rba
->w
, b_wa
);
2134 m3x3_mulv( ssrb
, rbb
->w
, b_wb
);
2135 v3_add( rba
->v
, b_wa
, b
);
2136 v3_sub( b
, rbb
->v
, b
);
2137 v3_sub( b
, b_wb
, b
);
2138 v3_muls( b
, -1.0f
, b
);
2141 m3x3_diagonal( invMa
, rba
->inv_mass
);
2142 m3x3_diagonal( invMb
, rbb
->inv_mass
);
2145 m3x3_mul( ssra
, rba
->iIw
, ia
);
2146 m3x3_mul( ia
, ssrat
, ia
);
2147 m3x3_mul( ssrb
, rbb
->iIw
, ib
);
2148 m3x3_mul( ib
, ssrbt
, ib
);
2151 m3x3_add( invMa
, ia
, cma
);
2152 m3x3_add( invMb
, ib
, cmb
);
2155 m3x3_add( cma
, cmb
, A
);
2157 /* Solve Ax = b ( A^-1*b = x ) */
2160 m3x3_inv( A
, invA
);
2161 m3x3_mulv( invA
, b
, impulse
);
2163 v3f delta_va
, delta_wa
, delta_vb
, delta_wb
;
2165 m3x3_mul( rba
->iIw
, ssrat
, iwa
);
2166 m3x3_mul( rbb
->iIw
, ssrbt
, iwb
);
2168 m3x3_mulv( invMa
, impulse
, delta_va
);
2169 m3x3_mulv( invMb
, impulse
, delta_vb
);
2170 m3x3_mulv( iwa
, impulse
, delta_wa
);
2171 m3x3_mulv( iwb
, impulse
, delta_wb
);
2173 v3_add( rba
->v
, delta_va
, rba
->v
);
2174 v3_add( rba
->w
, delta_wa
, rba
->w
);
2175 v3_sub( rbb
->v
, delta_vb
, rbb
->v
);
2176 v3_sub( rbb
->w
, delta_wb
, rbb
->w
);
2180 VG_STATIC
void rb_solve_swingtwist_constraints( rb_constr_swingtwist
*buf
,
2185 for( int i
=0; i
<len
; i
++ )
2187 rb_constr_swingtwist
*st
= &buf
[ i
];
2189 if( !st
->axis_violation
)
2192 float rv
= v3_dot( st
->axis
, st
->rbb
->w
) -
2193 v3_dot( st
->axis
, st
->rba
->w
);
2195 if( rv
* (float)st
->axis_violation
> 0.0f
)
2198 v3f impulse
, wa
, wb
;
2199 v3_muls( st
->axis
, rv
*st
->axis_mass
, impulse
);
2200 m3x3_mulv( st
->rba
->iIw
, impulse
, wa
);
2201 v3_add( st
->rba
->w
, wa
, st
->rba
->w
);
2203 v3_muls( impulse
, -1.0f
, impulse
);
2204 m3x3_mulv( st
->rbb
->iIw
, impulse
, wb
);
2205 v3_add( st
->rbb
->w
, wb
, st
->rbb
->w
);
2207 float rv2
= v3_dot( st
->axis
, st
->rbb
->w
) -
2208 v3_dot( st
->axis
, st
->rba
->w
);
2211 for( int i
=0; i
<len
; i
++ )
2213 rb_constr_swingtwist
*st
= &buf
[ i
];
2215 if( !st
->tangent_violation
)
2218 float rv
= v3_dot( st
->tangent_axis
, st
->rbb
->w
) -
2219 v3_dot( st
->tangent_axis
, st
->rba
->w
);
2224 v3f impulse
, wa
, wb
;
2225 v3_muls( st
->tangent_axis
, rv
*st
->tangent_mass
, impulse
);
2226 m3x3_mulv( st
->rba
->iIw
, impulse
, wa
);
2227 v3_add( st
->rba
->w
, wa
, st
->rba
->w
);
2229 v3_muls( impulse
, -1.0f
, impulse
);
2230 m3x3_mulv( st
->rbb
->iIw
, impulse
, wb
);
2231 v3_add( st
->rbb
->w
, wb
, st
->rbb
->w
);
2233 float rv2
= v3_dot( st
->tangent_axis
, st
->rbb
->w
) -
2234 v3_dot( st
->tangent_axis
, st
->rba
->w
);
2238 VG_STATIC
void rb_solve_constr_angle( rigidbody
*rba
, rigidbody
*rbb
,
2241 m3x3f ssra
, ssrb
, ssrat
, ssrbt
;
2244 m3x3_skew_symetric( ssrat
, ra
);
2245 m3x3_skew_symetric( ssrbt
, rb
);
2246 m3x3_transpose( ssrat
, ssra
);
2247 m3x3_transpose( ssrbt
, ssrb
);
2249 m3x3_mul( ssra
, rba
->iIw
, cma
);
2250 m3x3_mul( cma
, ssrat
, cma
);
2251 m3x3_mul( ssrb
, rbb
->iIw
, cmb
);
2252 m3x3_mul( cmb
, ssrbt
, cmb
);
2255 m3x3_add( cma
, cmb
, A
);
2256 m3x3_inv( A
, invA
);
2259 m3x3_mulv( ssra
, rba
->w
, b_wa
);
2260 m3x3_mulv( ssrb
, rbb
->w
, b_wb
);
2261 v3_add( b_wa
, b_wb
, b
);
2265 m3x3_mulv( invA
, b
, impulse
);
2267 v3f delta_wa
, delta_wb
;
2269 m3x3_mul( rba
->iIw
, ssrat
, iwa
);
2270 m3x3_mul( rbb
->iIw
, ssrbt
, iwb
);
2271 m3x3_mulv( iwa
, impulse
, delta_wa
);
2272 m3x3_mulv( iwb
, impulse
, delta_wb
);
2273 v3_add( rba
->w
, delta_wa
, rba
->w
);
2274 v3_sub( rbb
->w
, delta_wb
, rbb
->w
);
2278 * Correct position constraint drift errors
2279 * [ 0.0 <= amt <= 1.0 ]: the correction amount
2281 VG_STATIC
void rb_correct_position_constraints( rb_constr_pos
*buf
, int len
,
2284 for( int i
=0; i
<len
; i
++ )
2286 rb_constr_pos
*constr
= &buf
[i
];
2287 rigidbody
*rba
= constr
->rba
, *rbb
= constr
->rbb
;
2290 m3x3_mulv( rba
->to_world
, constr
->lca
, p0
);
2291 m3x3_mulv( rbb
->to_world
, constr
->lcb
, p1
);
2292 v3_add( rba
->co
, p0
, p0
);
2293 v3_add( rbb
->co
, p1
, p1
);
2294 v3_sub( p1
, p0
, d
);
2296 v3_muladds( rbb
->co
, d
, -1.0f
* amt
, rbb
->co
);
2297 rb_update_transform( rbb
);
2301 VG_STATIC
void rb_correct_swingtwist_constraints( rb_constr_swingtwist
*buf
,
2302 int len
, float amt
)
2304 for( int i
=0; i
<len
; i
++ )
2306 rb_constr_swingtwist
*st
= &buf
[i
];
2308 if( !st
->tangent_violation
)
2312 m3x3_mulv( st
->rbb
->to_world
, st
->coneva
, va
);
2314 float angle
= v3_dot( va
, st
->tangent_target
);
2316 if( fabsf(angle
) < 0.9999f
)
2319 v3_cross( va
, st
->tangent_target
, axis
);
2322 q_axis_angle( correction
, axis
, acosf(angle
) * amt
);
2323 q_mul( correction
, st
->rbb
->q
, st
->rbb
->q
);
2324 rb_update_transform( st
->rbb
);
2328 for( int i
=0; i
<len
; i
++ )
2330 rb_constr_swingtwist
*st
= &buf
[i
];
2332 if( !st
->axis_violation
)
2336 m3x3_mulv( st
->rbb
->to_world
, st
->conevxb
, vxb
);
2338 float angle
= v3_dot( vxb
, st
->axis_target
);
2340 if( fabsf(angle
) < 0.9999f
)
2343 v3_cross( vxb
, st
->axis_target
, axis
);
2346 q_axis_angle( correction
, axis
, acosf(angle
) * amt
);
2347 q_mul( correction
, st
->rbb
->q
, st
->rbb
->q
);
2348 rb_update_transform( st
->rbb
);
2353 VG_STATIC
void rb_correct_contact_constraints( rb_ct
*buf
, int len
, float amt
)
2355 for( int i
=0; i
<len
; i
++ )
2357 rb_ct
*ct
= &buf
[i
];
2358 rigidbody
*rba
= ct
->rba
,
2361 float mass_total
= 1.0f
/ (rba
->inv_mass
+ rbb
->inv_mass
);
2363 v3_muladds( rba
->co
, ct
->n
, -mass_total
* rba
->inv_mass
, rba
->co
);
2364 v3_muladds( rbb
->co
, ct
->n
, mass_total
* rbb
->inv_mass
, rbb
->co
);
2373 VG_STATIC
void rb_effect_simple_bouyency( rigidbody
*ra
, v4f plane
,
2374 float amt
, float drag
)
2377 float depth
= v3_dot( plane
, ra
->co
) - plane
[3],
2378 lambda
= vg_clampf( -depth
, 0.0f
, 1.0f
) * amt
;
2380 v3_muladds( ra
->v
, plane
, lambda
* k_rb_delta
, ra
->v
);
2383 v3_muls( ra
->v
, 1.0f
-(drag
*k_rb_delta
), ra
->v
);
2387 * -----------------------------------------------------------------------------
2388 * BVH implementation, this is ONLY for VG_STATIC rigidbodies, its to slow for
2390 * -----------------------------------------------------------------------------
2393 VG_STATIC
void rb_bh_expand_bound( void *user
, boxf bound
, u32 item_index
)
2395 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
2396 box_concat( bound
, rb
->bbx_world
);
2399 VG_STATIC
float rb_bh_centroid( void *user
, u32 item_index
, int axis
)
2401 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
2402 return (rb
->bbx_world
[axis
][0] + rb
->bbx_world
[1][axis
]) * 0.5f
;
2405 VG_STATIC
void rb_bh_swap( void *user
, u32 ia
, u32 ib
)
2407 rigidbody temp
, *rba
, *rbb
;
2408 rba
= &((rigidbody
*)user
)[ ia
];
2409 rbb
= &((rigidbody
*)user
)[ ib
];
2416 VG_STATIC
void rb_bh_debug( void *user
, u32 item_index
)
2418 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
2419 rb_debug( rb
, 0xff00ffff );
2422 VG_STATIC bh_system bh_system_rigidbodies
=
2424 .expand_bound
= rb_bh_expand_bound
,
2425 .item_centroid
= rb_bh_centroid
,
2426 .item_swap
= rb_bh_swap
,
2427 .item_debug
= rb_bh_debug
,
2431 #endif /* RIGIDBODY_H */