2 * Copyright (C) 2021-2022 Mt.ZERO Software, Harry Godden - All Rights Reserved
6 * Resources: Box2D - Erin Catto
17 VG_STATIC
void rb_tangent_basis( v3f n
, v3f tx
, v3f ty
);
18 VG_STATIC bh_system bh_system_rigidbodies
;
24 * -----------------------------------------------------------------------------
26 * -----------------------------------------------------------------------------
30 k_rb_rate
= (1.0/VG_TIMESTEP_FIXED
),
31 k_rb_delta
= (1.0/k_rb_rate
),
33 k_damp_linear
= 0.05f
, /* scale velocity 1/(1+x) */
34 k_damp_angular
= 0.1f
, /* scale angular 1/(1+x) */
36 k_joint_bias
= 0.08f
, /* positional joints */
37 k_joint_correction
= 0.01f
,
38 k_penetration_slop
= 0.01f
,
39 k_inertia_scale
= 4.0f
;
42 * -----------------------------------------------------------------------------
43 * structure definitions
44 * -----------------------------------------------------------------------------
47 typedef struct rigidbody rigidbody
;
48 typedef struct contact rb_ct
;
58 k_rb_shape_sphere
= 1,
59 k_rb_shape_capsule
= 2,
86 v3f right
, up
, forward
;
93 /* inertia model and inverse world tensor */
97 m4x3f to_world
, to_local
;
100 VG_STATIC
struct contact
102 rigidbody
*rba
, *rbb
;
105 float p
, bias
, norm_impulse
, tangent_impulse
[2],
106 normal_mass
, tangent_mass
[2];
110 enum contact_type type
;
112 rb_contact_buffer
[256];
113 VG_STATIC
int rb_contact_count
= 0;
116 * -----------------------------------------------------------------------------
118 * -----------------------------------------------------------------------------
121 VG_STATIC
float sphere_volume( float radius
)
123 float r3
= radius
*radius
*radius
;
124 return (4.0f
/3.0f
) * VG_PIf
* r3
;
127 VG_STATIC
void rb_tangent_basis( v3f n
, v3f tx
, v3f ty
)
129 /* Compute tangent basis (box2d) */
130 if( fabsf( n
[0] ) >= 0.57735027f
)
144 v3_cross( n
, tx
, ty
);
148 * -----------------------------------------------------------------------------
150 * -----------------------------------------------------------------------------
153 VG_STATIC
void rb_debug_contact( rb_ct
*ct
)
155 if( ct
->type
!= k_contact_type_disabled
)
158 v3_muladds( ct
->co
, ct
->n
, 0.1f
, p1
);
159 vg_line_pt3( ct
->co
, 0.025f
, 0xff0000ff );
160 vg_line( ct
->co
, p1
, 0xffffffff );
164 VG_STATIC
void debug_sphere( m4x3f m
, float radius
, u32 colour
)
166 v3f ly
= { 0.0f
, 0.0f
, radius
},
167 lx
= { 0.0f
, radius
, 0.0f
},
168 lz
= { 0.0f
, 0.0f
, radius
};
170 for( int i
=0; i
<16; i
++ )
172 float t
= ((float)(i
+1) * (1.0f
/16.0f
)) * VG_PIf
* 2.0f
,
176 v3f py
= { s
*radius
, 0.0f
, c
*radius
},
177 px
= { s
*radius
, c
*radius
, 0.0f
},
178 pz
= { 0.0f
, s
*radius
, c
*radius
};
180 v3f p0
, p1
, p2
, p3
, p4
, p5
;
181 m4x3_mulv( m
, py
, p0
);
182 m4x3_mulv( m
, ly
, p1
);
183 m4x3_mulv( m
, px
, p2
);
184 m4x3_mulv( m
, lx
, p3
);
185 m4x3_mulv( m
, pz
, p4
);
186 m4x3_mulv( m
, lz
, p5
);
188 vg_line( p0
, p1
, colour
== 0x00? 0xff00ff00: colour
);
189 vg_line( p2
, p3
, colour
== 0x00? 0xff0000ff: colour
);
190 vg_line( p4
, p5
, colour
== 0x00? 0xffff0000: colour
);
198 VG_STATIC
void debug_capsule( m4x3f m
, float radius
, float h
, u32 colour
)
200 v3f ly
= { 0.0f
, 0.0f
, radius
},
201 lx
= { 0.0f
, radius
, 0.0f
},
202 lz
= { 0.0f
, 0.0f
, radius
};
204 float s0
= sinf(0.0f
)*radius
,
205 c0
= cosf(0.0f
)*radius
;
207 v3f p0
, p1
, up
, right
, forward
;
208 m3x3_mulv( m
, (v3f
){0.0f
,1.0f
,0.0f
}, up
);
209 m3x3_mulv( m
, (v3f
){1.0f
,0.0f
,0.0f
}, right
);
210 m3x3_mulv( m
, (v3f
){0.0f
,0.0f
,-1.0f
}, forward
);
211 v3_muladds( m
[3], up
, -h
*0.5f
+radius
, p0
);
212 v3_muladds( m
[3], up
, h
*0.5f
-radius
, p1
);
215 v3_muladds( p0
, right
, radius
, a0
);
216 v3_muladds( p1
, right
, radius
, a1
);
217 v3_muladds( p0
, forward
, radius
, b0
);
218 v3_muladds( p1
, forward
, radius
, b1
);
219 vg_line( a0
, a1
, colour
);
220 vg_line( b0
, b1
, colour
);
222 v3_muladds( p0
, right
, -radius
, a0
);
223 v3_muladds( p1
, right
, -radius
, a1
);
224 v3_muladds( p0
, forward
, -radius
, b0
);
225 v3_muladds( p1
, forward
, -radius
, b1
);
226 vg_line( a0
, a1
, colour
);
227 vg_line( b0
, b1
, colour
);
229 for( int i
=0; i
<16; i
++ )
231 float t
= ((float)(i
+1) * (1.0f
/16.0f
)) * VG_PIf
* 2.0f
,
235 v3f e0
= { s0
, 0.0f
, c0
},
236 e1
= { s1
, 0.0f
, c1
},
237 e2
= { s0
, c0
, 0.0f
},
238 e3
= { s1
, c1
, 0.0f
},
239 e4
= { 0.0f
, c0
, s0
},
240 e5
= { 0.0f
, c1
, s1
};
242 m3x3_mulv( m
, e0
, e0
);
243 m3x3_mulv( m
, e1
, e1
);
244 m3x3_mulv( m
, e2
, e2
);
245 m3x3_mulv( m
, e3
, e3
);
246 m3x3_mulv( m
, e4
, e4
);
247 m3x3_mulv( m
, e5
, e5
);
249 v3_add( p0
, e0
, a0
);
250 v3_add( p0
, e1
, a1
);
251 v3_add( p1
, e0
, b0
);
252 v3_add( p1
, e1
, b1
);
254 vg_line( a0
, a1
, colour
);
255 vg_line( b0
, b1
, colour
);
259 v3_add( p0
, e2
, a0
);
260 v3_add( p0
, e3
, a1
);
261 v3_add( p0
, e4
, b0
);
262 v3_add( p0
, e5
, b1
);
266 v3_add( p1
, e2
, a0
);
267 v3_add( p1
, e3
, a1
);
268 v3_add( p1
, e4
, b0
);
269 v3_add( p1
, e5
, b1
);
272 vg_line( a0
, a1
, colour
);
273 vg_line( b0
, b1
, colour
);
280 VG_STATIC
void rb_debug( rigidbody
*rb
, u32 colour
)
282 if( rb
->type
== k_rb_shape_box
)
285 vg_line_boxf_transformed( rb
->to_world
, rb
->bbx
, colour
);
287 else if( rb
->type
== k_rb_shape_sphere
)
289 debug_sphere( rb
->to_world
, rb
->inf
.sphere
.radius
, colour
);
291 else if( rb
->type
== k_rb_shape_capsule
)
294 float h
= rb
->inf
.capsule
.height
,
295 r
= rb
->inf
.capsule
.radius
;
297 debug_capsule( rb
->to_world
, r
, h
, colour
);
299 else if( rb
->type
== k_rb_shape_scene
)
301 vg_line_boxf( rb
->bbx
, colour
);
306 * -----------------------------------------------------------------------------
308 * -----------------------------------------------------------------------------
312 * Update world space bounding box based on local one
314 VG_STATIC
void rb_update_bounds( rigidbody
*rb
)
316 box_copy( rb
->bbx
, rb
->bbx_world
);
317 m4x3_transform_aabb( rb
->to_world
, rb
->bbx_world
);
321 * Commit transform to rigidbody. Updates matrices
323 VG_STATIC
void rb_update_transform( rigidbody
*rb
)
325 q_normalize( rb
->q
);
326 q_m3x3( rb
->q
, rb
->to_world
);
327 v3_copy( rb
->co
, rb
->to_world
[3] );
329 m4x3_invert_affine( rb
->to_world
, rb
->to_local
);
331 m3x3_mulv( rb
->to_world
, (v3f
){1.0f
,0.0f
, 0.0f
}, rb
->right
);
332 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,1.0f
, 0.0f
}, rb
->up
);
333 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,0.0f
,-1.0f
}, rb
->forward
);
335 m3x3_mul( rb
->iI
, rb
->to_local
, rb
->iIw
);
336 m3x3_mul( rb
->to_world
, rb
->iIw
, rb
->iIw
);
338 rb_update_bounds( rb
);
342 * Extrapolate rigidbody into a transform based on vg accumulator.
343 * Useful for rendering
345 VG_STATIC
void rb_extrapolate_transform( rigidbody
*rb
, m4x3f transform
)
347 float substep
= vg_clampf( vg
.accumulator
/ k_rb_delta
, 0.0f
, 1.0f
);
352 v3_muladds( rb
->co
, rb
->v
, k_rb_delta
*substep
, co
);
354 if( v3_length2( rb
->w
) > 0.0f
)
358 v3_copy( rb
->w
, axis
);
360 float mag
= v3_length( axis
);
361 v3_divs( axis
, mag
, axis
);
362 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
*substep
);
363 q_mul( rotation
, rb
->q
, q
);
371 q_m3x3( q
, transform
);
372 v3_copy( co
, transform
[3] );
376 * Initialize rigidbody and calculate masses, inertia
378 VG_STATIC
void rb_init( rigidbody
*rb
)
382 if( rb
->type
== k_rb_shape_box
)
385 v3_sub( rb
->bbx
[1], rb
->bbx
[0], dims
);
386 volume
= dims
[0]*dims
[1]*dims
[2];
388 else if( rb
->type
== k_rb_shape_sphere
)
390 volume
= sphere_volume( rb
->inf
.sphere
.radius
);
391 v3_fill( rb
->bbx
[0], -rb
->inf
.sphere
.radius
);
392 v3_fill( rb
->bbx
[1], rb
->inf
.sphere
.radius
);
394 else if( rb
->type
== k_rb_shape_capsule
)
396 float r
= rb
->inf
.capsule
.radius
,
397 h
= rb
->inf
.capsule
.height
;
398 volume
= sphere_volume( r
) + VG_PIf
* r
*r
* (h
- r
*2.0f
);
400 v3_fill( rb
->bbx
[0], -rb
->inf
.sphere
.radius
);
401 v3_fill( rb
->bbx
[1], rb
->inf
.sphere
.radius
);
405 else if( rb
->type
== k_rb_shape_scene
)
408 box_copy( rb
->inf
.scene
.bh_scene
->nodes
[0].bbx
, rb
->bbx
);
419 float mass
= 2.0f
*volume
;
420 rb
->inv_mass
= 1.0f
/mass
;
423 v3_sub( rb
->bbx
[1], rb
->bbx
[0], extent
);
424 v3_muls( extent
, 0.5f
, extent
);
426 /* local intertia tensor */
427 float scale
= k_inertia_scale
;
428 float ex2
= scale
*extent
[0]*extent
[0],
429 ey2
= scale
*extent
[1]*extent
[1],
430 ez2
= scale
*extent
[2]*extent
[2];
432 rb
->I
[0] = ((1.0f
/12.0f
) * mass
* (ey2
+ez2
));
433 rb
->I
[1] = ((1.0f
/12.0f
) * mass
* (ex2
+ez2
));
434 rb
->I
[2] = ((1.0f
/12.0f
) * mass
* (ex2
+ey2
));
436 m3x3_identity( rb
->iI
);
437 rb
->iI
[0][0] = rb
->I
[0];
438 rb
->iI
[1][1] = rb
->I
[1];
439 rb
->iI
[2][2] = rb
->I
[2];
440 m3x3_inv( rb
->iI
, rb
->iI
);
446 rb_update_transform( rb
);
449 VG_STATIC
void rb_iter( rigidbody
*rb
)
451 if( !vg_validf( rb
->v
[0] ) ||
452 !vg_validf( rb
->v
[1] ) ||
453 !vg_validf( rb
->v
[2] ) )
455 vg_fatal_exit_loop( "NaN velocity" );
458 v3f gravity
= { 0.0f
, -9.8f
, 0.0f
};
459 v3_muladds( rb
->v
, gravity
, k_rb_delta
, rb
->v
);
461 /* intergrate velocity */
462 v3_muladds( rb
->co
, rb
->v
, k_rb_delta
, rb
->co
);
463 v3_lerp( rb
->w
, (v3f
){0.0f
,0.0f
,0.0f
}, 0.0025f
, rb
->w
);
465 /* inegrate inertia */
466 if( v3_length2( rb
->w
) > 0.0f
)
470 v3_copy( rb
->w
, axis
);
472 float mag
= v3_length( axis
);
473 v3_divs( axis
, mag
, axis
);
474 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
);
475 q_mul( rotation
, rb
->q
, rb
->q
);
479 v3_muls( rb
->v
, 1.0f
/(1.0f
+k_rb_delta
*k_damp_linear
), rb
->v
);
480 v3_muls( rb
->w
, 1.0f
/(1.0f
+k_rb_delta
*k_damp_angular
), rb
->w
);
485 * -----------------------------------------------------------------------------
486 * Boolean shape overlap functions
487 * -----------------------------------------------------------------------------
491 * Project AABB, and triangle interval onto axis to check if they overlap
493 VG_STATIC
int rb_box_triangle_interval( v3f extent
, v3f axis
, v3f tri
[3] )
497 r
= extent
[0] * fabsf(axis
[0]) +
498 extent
[1] * fabsf(axis
[1]) +
499 extent
[2] * fabsf(axis
[2]),
501 p0
= v3_dot( axis
, tri
[0] ),
502 p1
= v3_dot( axis
, tri
[1] ),
503 p2
= v3_dot( axis
, tri
[2] ),
505 e
= vg_maxf(-vg_maxf(p0
,vg_maxf(p1
,p2
)), vg_minf(p0
,vg_minf(p1
,p2
)));
507 if( e
> r
) return 0;
512 * Seperating axis test box vs triangle
514 VG_STATIC
int rb_box_triangle_sat( rigidbody
*rba
, v3f tri_src
[3] )
519 v3_sub( rba
->bbx
[1], rba
->bbx
[0], extent
);
520 v3_muls( extent
, 0.5f
, extent
);
521 v3_add( rba
->bbx
[0], extent
, c
);
523 for( int i
=0; i
<3; i
++ )
525 m4x3_mulv( rba
->to_local
, tri_src
[i
], tri
[i
] );
526 v3_sub( tri
[i
], c
, tri
[i
] );
530 if(!rb_box_triangle_interval( extent
, (v3f
){1.0f
,0.0f
,0.0f
}, tri
)) return 0;
531 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,1.0f
,0.0f
}, tri
)) return 0;
532 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,0.0f
,1.0f
}, tri
)) return 0;
534 v3f v0
,v1
,v2
,n
, e0
,e1
,e2
;
535 v3_sub( tri
[1], tri
[0], v0
);
536 v3_sub( tri
[2], tri
[0], v1
);
537 v3_sub( tri
[2], tri
[1], v2
);
541 v3_cross( v0
, v1
, n
);
542 v3_cross( v0
, n
, e0
);
543 v3_cross( n
, v1
, e1
);
544 v3_cross( v2
, n
, e2
);
547 if(!rb_box_triangle_interval( extent
, n
, tri
)) return 0;
550 v3_cross( e0
, (v3f
){1.0f
,0.0f
,0.0f
}, axis
[0] );
551 v3_cross( e0
, (v3f
){0.0f
,1.0f
,0.0f
}, axis
[1] );
552 v3_cross( e0
, (v3f
){0.0f
,0.0f
,1.0f
}, axis
[2] );
553 v3_cross( e1
, (v3f
){1.0f
,0.0f
,0.0f
}, axis
[3] );
554 v3_cross( e1
, (v3f
){0.0f
,1.0f
,0.0f
}, axis
[4] );
555 v3_cross( e1
, (v3f
){0.0f
,0.0f
,1.0f
}, axis
[5] );
556 v3_cross( e2
, (v3f
){1.0f
,0.0f
,0.0f
}, axis
[6] );
557 v3_cross( e2
, (v3f
){0.0f
,1.0f
,0.0f
}, axis
[7] );
558 v3_cross( e2
, (v3f
){0.0f
,0.0f
,1.0f
}, axis
[8] );
560 for( int i
=0; i
<9; i
++ )
561 if(!rb_box_triangle_interval( extent
, axis
[i
], tri
)) return 0;
567 * -----------------------------------------------------------------------------
569 * -----------------------------------------------------------------------------
572 VG_STATIC
int rb_manifold_apply_filtered( rb_ct
*man
, int len
)
576 for( int i
=0; i
<len
; i
++ )
580 if( ct
->type
== k_contact_type_disabled
)
590 * Merge two contacts if they are within radius(r) of eachother
592 VG_STATIC
void rb_manifold_contact_weld( rb_ct
*ci
, rb_ct
*cj
, float r
)
594 if( v3_dist2( ci
->co
, cj
->co
) < r
*r
)
596 cj
->type
= k_contact_type_disabled
;
597 ci
->p
= (ci
->p
+ cj
->p
) * 0.5f
;
599 v3_add( ci
->co
, cj
->co
, ci
->co
);
600 v3_muls( ci
->co
, 0.5f
, ci
->co
);
603 v3_sub( ci
->rba
->co
, ci
->co
, delta
);
605 float c0
= v3_dot( ci
->n
, delta
),
606 c1
= v3_dot( cj
->n
, delta
);
608 if( c0
< 0.0f
|| c1
< 0.0f
)
611 ci
->type
= k_contact_type_disabled
;
616 v3_muls( ci
->n
, c0
, n
);
617 v3_muladds( n
, cj
->n
, c1
, n
);
627 VG_STATIC
void rb_manifold_filter_joint_edges( rb_ct
*man
, int len
, float r
)
629 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
++ )
638 if( cj
->type
!= k_contact_type_edge
)
641 rb_manifold_contact_weld( ci
, cj
, r
);
647 * Resolve overlapping pairs
651 VG_STATIC
void rb_manifold_filter_pairs( rb_ct
*man
, int len
, float r
)
653 for( int i
=0; i
<len
-1; i
++ )
658 if( ci
->type
== k_contact_type_disabled
) continue;
660 for( int j
=i
+1; j
<len
; j
++ )
664 if( cj
->type
== k_contact_type_disabled
) continue;
666 if( v3_dist2( ci
->co
, cj
->co
) < r
*r
)
668 cj
->type
= k_contact_type_disabled
;
669 v3_add( cj
->n
, ci
->n
, ci
->n
);
677 float n
= 1.0f
/((float)similar
+1.0f
);
678 v3_muls( ci
->n
, n
, ci
->n
);
681 if( v3_length2(ci
->n
) < 0.1f
*0.1f
)
682 ci
->type
= k_contact_type_disabled
;
684 v3_normalize( ci
->n
);
690 * Remove contacts that are facing away from A
692 VG_STATIC
void rb_manifold_filter_backface( rb_ct
*man
, int len
)
694 for( int i
=0; i
<len
; i
++ )
697 if( ct
->type
== k_contact_type_disabled
)
701 v3_sub( ct
->co
, ct
->rba
->co
, delta
);
703 if( v3_dot( delta
, ct
->n
) > -0.001f
)
704 ct
->type
= k_contact_type_disabled
;
709 * Filter out duplicate coplanar results. Good for spheres.
711 VG_STATIC
void rb_manifold_filter_coplanar( rb_ct
*man
, int len
, float w
)
713 for( int i
=0; i
<len
; i
++ )
716 if( ci
->type
== k_contact_type_disabled
||
717 ci
->type
== k_contact_type_edge
)
720 float d1
= v3_dot( ci
->co
, ci
->n
);
722 for( int j
=0; j
<len
; j
++ )
728 if( cj
->type
== k_contact_type_disabled
)
731 float d2
= v3_dot( cj
->co
, ci
->n
),
734 if( fabsf( d
) <= w
)
736 cj
->type
= k_contact_type_disabled
;
743 * -----------------------------------------------------------------------------
745 * -----------------------------------------------------------------------------
751 * These do not automatically allocate contacts, an appropriately sized
752 * buffer must be supplied. The function returns the size of the manifold
753 * which was generated.
755 * The values set on the contacts are: n, co, p, rba, rbb
759 * By collecting the minimum(time) and maximum(time) pairs of points, we
760 * build a reduced and stable exact manifold.
763 * rx: minimum distance of these points
764 * dx: the delta between the two points
766 * pairs will only ammend these if they are creating a collision
768 typedef struct capsule_manifold capsule_manifold
;
769 struct capsule_manifold
777 * Expand a line manifold with a new pair. t value is the time along segment
778 * on the oriented object which created this pair.
780 VG_STATIC
void rb_capsule_manifold( v3f pa
, v3f pb
, float t
, float r
,
781 capsule_manifold
*manifold
)
784 v3_sub( pa
, pb
, delta
);
786 if( v3_length2(delta
) < r
*r
)
788 if( t
< manifold
->t0
)
790 v3_copy( delta
, manifold
->d0
);
795 if( t
> manifold
->t1
)
797 v3_copy( delta
, manifold
->d1
);
804 VG_STATIC
void rb_capsule_manifold_init( capsule_manifold
*manifold
)
806 manifold
->t0
= INFINITY
;
807 manifold
->t1
= -INFINITY
;
810 VG_STATIC
int rb_capsule_manifold_done( rigidbody
*rba
, rigidbody
*rbb
,
811 capsule_manifold
*manifold
, rb_ct
*buf
)
813 float h
= rba
->inf
.capsule
.height
,
814 ra
= rba
->inf
.capsule
.radius
;
817 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+ra
, p0
);
818 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-ra
, p1
);
821 if( manifold
->t0
<= 1.0f
)
826 v3_muls( p0
, 1.0f
-manifold
->t0
, pa
);
827 v3_muladds( pa
, p1
, manifold
->t0
, pa
);
829 float d
= v3_length( manifold
->d0
);
830 v3_muls( manifold
->d0
, 1.0f
/d
, ct
->n
);
831 v3_muladds( pa
, ct
->n
, -ra
, ct
->co
);
833 ct
->p
= manifold
->r0
- d
;
836 ct
->type
= k_contact_type_default
;
841 if( (manifold
->t1
>= 0.0f
) && (manifold
->t0
!= manifold
->t1
) )
843 rb_ct
*ct
= buf
+count
;
846 v3_muls( p0
, 1.0f
-manifold
->t1
, pa
);
847 v3_muladds( pa
, p1
, manifold
->t1
, pa
);
849 float d
= v3_length( manifold
->d1
);
850 v3_muls( manifold
->d1
, 1.0f
/d
, ct
->n
);
851 v3_muladds( pa
, ct
->n
, -ra
, ct
->co
);
853 ct
->p
= manifold
->r1
- d
;
856 ct
->type
= k_contact_type_default
;
866 vg_line( buf
[0].co
, buf
[1].co
, 0xff0000ff );
871 VG_STATIC
int rb_capsule_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
873 float h
= rba
->inf
.capsule
.height
,
874 ra
= rba
->inf
.capsule
.radius
,
875 rb
= rbb
->inf
.sphere
.radius
;
878 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+ra
, p0
);
879 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-ra
, p1
);
882 closest_point_segment( p0
, p1
, rbb
->co
, c
);
883 v3_sub( c
, rbb
->co
, delta
);
885 float d2
= v3_length2(delta
),
893 v3_muls( delta
, 1.0f
/d
, ct
->n
);
897 v3_muladds( c
, ct
->n
, -ra
, p0
);
898 v3_muladds( rbb
->co
, ct
->n
, rb
, p1
);
899 v3_add( p0
, p1
, ct
->co
);
900 v3_muls( ct
->co
, 0.5f
, ct
->co
);
904 ct
->type
= k_contact_type_default
;
912 VG_STATIC
int rb_capsule_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
914 float ha
= rba
->inf
.capsule
.height
,
915 hb
= rbb
->inf
.capsule
.height
,
916 ra
= rba
->inf
.capsule
.radius
,
917 rb
= rbb
->inf
.capsule
.radius
,
921 v3_muladds( rba
->co
, rba
->up
, -ha
*0.5f
+ra
, p0
);
922 v3_muladds( rba
->co
, rba
->up
, ha
*0.5f
-ra
, p1
);
923 v3_muladds( rbb
->co
, rbb
->up
, -hb
*0.5f
+rb
, p2
);
924 v3_muladds( rbb
->co
, rbb
->up
, hb
*0.5f
-rb
, p3
);
926 capsule_manifold manifold
;
927 rb_capsule_manifold_init( &manifold
);
931 closest_segment_segment( p0
, p1
, p2
, p3
, &ta
, &tb
, pa
, pb
);
932 rb_capsule_manifold( pa
, pb
, ta
, r
, &manifold
);
934 ta
= closest_point_segment( p0
, p1
, p2
, pa
);
935 tb
= closest_point_segment( p0
, p1
, p3
, pb
);
936 rb_capsule_manifold( pa
, p2
, ta
, r
, &manifold
);
937 rb_capsule_manifold( pb
, p3
, tb
, r
, &manifold
);
939 closest_point_segment( p2
, p3
, p0
, pa
);
940 closest_point_segment( p2
, p3
, p1
, pb
);
941 rb_capsule_manifold( p0
, pa
, 0.0f
, r
, &manifold
);
942 rb_capsule_manifold( p1
, pb
, 1.0f
, r
, &manifold
);
944 return rb_capsule_manifold_done( rba
, rbb
, &manifold
, buf
);
948 * Generates up to two contacts; optimised for the most stable manifold
950 VG_STATIC
int rb_capsule_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
952 float h
= rba
->inf
.capsule
.height
,
953 r
= rba
->inf
.capsule
.radius
;
956 * Solving this in symetric local space of the cube saves us some time and a
957 * couple branches when it comes to the quad stage.
960 v3_add( rbb
->bbx
[0], rbb
->bbx
[1], centroid
);
961 v3_muls( centroid
, 0.5f
, centroid
);
964 v3_sub( rbb
->bbx
[0], centroid
, bbx
[0] );
965 v3_sub( rbb
->bbx
[1], centroid
, bbx
[1] );
967 v3f pc
, p0w
, p1w
, p0
, p1
;
968 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+r
, p0w
);
969 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-r
, p1w
);
971 m4x3_mulv( rbb
->to_local
, p0w
, p0
);
972 m4x3_mulv( rbb
->to_local
, p1w
, p1
);
973 v3_sub( p0
, centroid
, p0
);
974 v3_sub( p1
, centroid
, p1
);
975 v3_add( p0
, p1
, pc
);
976 v3_muls( pc
, 0.5f
, pc
);
979 * Finding an appropriate quad to collide lines with
982 v3_div( pc
, bbx
[1], region
);
985 if( (fabsf(region
[0]) > fabsf(region
[1])) &&
986 (fabsf(region
[0]) > fabsf(region
[2])) )
988 float px
= vg_signf(region
[0]) * bbx
[1][0];
989 v3_copy( (v3f
){ px
, bbx
[0][1], bbx
[0][2] }, quad
[0] );
990 v3_copy( (v3f
){ px
, bbx
[1][1], bbx
[0][2] }, quad
[1] );
991 v3_copy( (v3f
){ px
, bbx
[1][1], bbx
[1][2] }, quad
[2] );
992 v3_copy( (v3f
){ px
, bbx
[0][1], bbx
[1][2] }, quad
[3] );
994 else if( fabsf(region
[1]) > fabsf(region
[2]) )
996 float py
= vg_signf(region
[1]) * bbx
[1][1];
997 v3_copy( (v3f
){ bbx
[0][0], py
, bbx
[0][2] }, quad
[0] );
998 v3_copy( (v3f
){ bbx
[1][0], py
, bbx
[0][2] }, quad
[1] );
999 v3_copy( (v3f
){ bbx
[1][0], py
, bbx
[1][2] }, quad
[2] );
1000 v3_copy( (v3f
){ bbx
[0][0], py
, bbx
[1][2] }, quad
[3] );
1004 float pz
= vg_signf(region
[2]) * bbx
[1][2];
1005 v3_copy( (v3f
){ bbx
[0][0], bbx
[0][1], pz
}, quad
[0] );
1006 v3_copy( (v3f
){ bbx
[1][0], bbx
[0][1], pz
}, quad
[1] );
1007 v3_copy( (v3f
){ bbx
[1][0], bbx
[1][1], pz
}, quad
[2] );
1008 v3_copy( (v3f
){ bbx
[0][0], bbx
[1][1], pz
}, quad
[3] );
1011 capsule_manifold manifold
;
1012 rb_capsule_manifold_init( &manifold
);
1015 closest_point_aabb( p0
, bbx
, c0
);
1016 closest_point_aabb( p1
, bbx
, c1
);
1019 v3_sub( c0
, p0
, d0
);
1020 v3_sub( c1
, p1
, d1
);
1021 v3_sub( p1
, p0
, da
);
1027 if( v3_dot( da
, d0
) <= 0.01f
)
1028 rb_capsule_manifold( p0
, c0
, 0.0f
, r
, &manifold
);
1030 if( v3_dot( da
, d1
) >= -0.01f
)
1031 rb_capsule_manifold( p1
, c1
, 1.0f
, r
, &manifold
);
1033 for( int i
=0; i
<4; i
++ )
1040 closest_segment_segment( p0
, p1
, quad
[i0
], quad
[i1
], &ta
, &tb
, ca
, cb
);
1041 rb_capsule_manifold( ca
, cb
, ta
, r
, &manifold
);
1045 * Create final contacts based on line manifold
1047 m3x3_mulv( rbb
->to_world
, manifold
.d0
, manifold
.d0
);
1048 m3x3_mulv( rbb
->to_world
, manifold
.d1
, manifold
.d1
);
1055 for( int i
=0; i
<4; i
++ )
1061 v3_add( quad
[i0
], centroid
, q0
);
1062 v3_add( quad
[i1
], centroid
, q1
);
1064 m4x3_mulv( rbb
->to_world
, q0
, q0
);
1065 m4x3_mulv( rbb
->to_world
, q1
, q1
);
1067 vg_line( q0
, q1
, 0xffffffff );
1071 return rb_capsule_manifold_done( rba
, rbb
, &manifold
, buf
);
1074 VG_STATIC
int rb_sphere_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1078 closest_point_obb( rba
->co
, rbb
->bbx
, rbb
->to_world
, rbb
->to_local
, co
);
1079 v3_sub( rba
->co
, co
, delta
);
1081 float d2
= v3_length2(delta
),
1082 r
= rba
->inf
.sphere
.radius
;
1091 v3_sub( rba
->co
, rbb
->co
, delta
);
1094 * some extra testing is required to find the best axis to push the
1095 * object back outside the box. Since there isnt a clear seperating
1096 * vector already, especially on really high aspect boxes.
1098 float lx
= v3_dot( rbb
->right
, delta
),
1099 ly
= v3_dot( rbb
->up
, delta
),
1100 lz
= v3_dot( rbb
->forward
, delta
),
1101 px
= rbb
->bbx
[1][0] - fabsf(lx
),
1102 py
= rbb
->bbx
[1][1] - fabsf(ly
),
1103 pz
= rbb
->bbx
[1][2] - fabsf(lz
);
1105 if( px
< py
&& px
< pz
)
1106 v3_muls( rbb
->right
, vg_signf(lx
), ct
->n
);
1108 v3_muls( rbb
->up
, vg_signf(ly
), ct
->n
);
1110 v3_muls( rbb
->forward
, vg_signf(lz
), ct
->n
);
1112 v3_muladds( rba
->co
, ct
->n
, -r
, ct
->co
);
1118 v3_muls( delta
, 1.0f
/d
, ct
->n
);
1120 v3_copy( co
, ct
->co
);
1125 ct
->type
= k_contact_type_default
;
1132 VG_STATIC
int rb_sphere_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1135 v3_sub( rba
->co
, rbb
->co
, delta
);
1137 float d2
= v3_length2(delta
),
1138 r
= rba
->inf
.sphere
.radius
+ rbb
->inf
.sphere
.radius
;
1142 float d
= sqrtf(d2
);
1145 v3_muls( delta
, 1.0f
/d
, ct
->n
);
1148 v3_muladds( rba
->co
, ct
->n
,-rba
->inf
.sphere
.radius
, p0
);
1149 v3_muladds( rbb
->co
, ct
->n
, rbb
->inf
.sphere
.radius
, p1
);
1150 v3_add( p0
, p1
, ct
->co
);
1151 v3_muls( ct
->co
, 0.5f
, ct
->co
);
1152 ct
->type
= k_contact_type_default
;
1162 //#define RIGIDBODY_DYNAMIC_MESH_EDGES
1164 VG_STATIC
int rb_sphere_triangle( rigidbody
*rba
, rigidbody
*rbb
,
1165 v3f tri
[3], rb_ct
*buf
)
1169 #ifdef RIGIDBODY_DYNAMIC_MESH_EDGES
1170 closest_on_triangle_1( rba
->co
, tri
, co
);
1172 enum contact_type type
= closest_on_triangle_1( rba
->co
, tri
, co
);
1175 v3_sub( rba
->co
, co
, delta
);
1177 float d2
= v3_length2( delta
),
1178 r
= rba
->inf
.sphere
.radius
;
1185 v3_sub( tri
[2], tri
[0], ab
);
1186 v3_sub( tri
[1], tri
[0], ac
);
1187 v3_cross( ac
, ab
, tn
);
1188 v3_copy( tn
, ct
->n
);
1190 if( v3_length2( ct
->n
) <= 0.00001f
)
1192 vg_error( "Zero area triangle!\n" );
1196 v3_normalize( ct
->n
);
1198 float d
= sqrtf(d2
);
1200 v3_copy( co
, ct
->co
);
1212 VG_STATIC
void rb_debug_sharp_scene_edges( rigidbody
*rbb
, float sharp_ang
,
1213 boxf box
, u32 colour
)
1215 sharp_ang
= cosf( sharp_ang
);
1217 scene
*sc
= rbb
->inf
.scene
.bh_scene
->user
;
1218 vg_line_boxf( box
, 0xff00ff00 );
1221 bh_iter_init( 0, &it
);
1224 while( bh_next( rbb
->inf
.scene
.bh_scene
, &it
, box
, &idx
) )
1226 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
1229 for( int j
=0; j
<3; j
++ )
1230 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
1232 for( int j
=0; j
<3; j
++ )
1236 v3_sub( tri
[(j
+1)%3], tri
[j
], edir
);
1238 if( v3_dot( edir
, (v3f
){ 0.5184758473652127f
,
1239 0.2073903389460850f
,
1240 -0.8295613557843402f
} ) < 0.0f
)
1245 bh_iter_init( 0, &jt
);
1248 float const k_r
= 0.02f
;
1249 v3_add( (v3f
){ k_r
, k_r
, k_r
}, tri
[j
], region
[1] );
1250 v3_add( (v3f
){ -k_r
, -k_r
, -k_r
}, tri
[j
], region
[0] );
1253 while( bh_next( rbb
->inf
.scene
.bh_scene
, &jt
, region
, &jdx
) )
1258 u32
*ptrj
= &sc
->arrindices
[ jdx
*3 ];
1261 for( int k
=0; k
<3; k
++ )
1262 v3_copy( sc
->arrvertices
[ptrj
[k
]].co
, trj
[k
] );
1264 for( int k
=0; k
<3; k
++ )
1266 if( v3_dist2( tri
[j
], trj
[k
] ) <= k_r
*k_r
)
1273 if( v3_dist2( tri
[jp1
], trj
[km1
] ) <= k_r
*k_r
)
1276 v3_sub( tri
[jp1
], tri
[j
], b0
);
1277 v3_sub( tri
[jp2
], tri
[j
], b1
);
1278 v3_sub( trj
[km2
], tri
[j
], b2
);
1281 v3_cross( b0
, b1
, cx0
);
1282 v3_cross( b2
, b0
, cx1
);
1284 float polarity
= v3_dot( cx0
, b2
);
1286 if( polarity
< 0.0f
)
1289 vg_line( tri
[j
], tri
[jp1
], 0xff00ff00 );
1290 float ang
= v3_dot(cx0
,cx1
) /
1291 (v3_length(cx0
)*v3_length(cx1
));
1292 if( ang
< sharp_ang
)
1294 vg_line( tri
[j
], tri
[jp1
], 0xff00ff00 );
1306 VG_STATIC
int rb_sphere_scene( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1308 scene
*sc
= rbb
->inf
.scene
.bh_scene
->user
;
1311 bh_iter_init( 0, &it
);
1316 while( bh_next( rbb
->inf
.scene
.bh_scene
, &it
, rba
->bbx_world
, &idx
) )
1318 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
1321 for( int j
=0; j
<3; j
++ )
1322 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
1324 buf
[ count
].element_id
= ptri
[0];
1326 vg_line( tri
[0],tri
[1],0x70ff6000 );
1327 vg_line( tri
[1],tri
[2],0x70ff6000 );
1328 vg_line( tri
[2],tri
[0],0x70ff6000 );
1330 int contact
= rb_sphere_triangle( rba
, rbb
, tri
, buf
+count
);
1335 vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
1343 VG_STATIC
int rb_box_scene( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1345 scene
*sc
= rbb
->inf
.scene
.bh_scene
->user
;
1350 bh_iter_init( 0, &it
);
1355 while( bh_next( rbb
->inf
.scene
.bh_scene
, &it
, rba
->bbx_world
, &idx
) )
1357 u32
*ptri
= &sc
->arrindices
[ idx
*3 ];
1359 for( int j
=0; j
<3; j
++ )
1360 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
1362 if( rb_box_triangle_sat( rba
, tri
) )
1364 vg_line(tri
[0],tri
[1],0xff50ff00 );
1365 vg_line(tri
[1],tri
[2],0xff50ff00 );
1366 vg_line(tri
[2],tri
[0],0xff50ff00 );
1370 vg_line(tri
[0],tri
[1],0xff0000ff );
1371 vg_line(tri
[1],tri
[2],0xff0000ff );
1372 vg_line(tri
[2],tri
[0],0xff0000ff );
1378 v3_sub( tri
[1], tri
[0], v0
);
1379 v3_sub( tri
[2], tri
[0], v1
);
1380 v3_cross( v0
, v1
, n
);
1383 /* find best feature */
1384 float best
= v3_dot( rba
->right
, n
);
1387 float cy
= v3_dot( rba
->up
, n
);
1388 if( fabsf(cy
) > fabsf(best
) )
1394 float cz
= -v3_dot( rba
->forward
, n
);
1395 if( fabsf(cz
) > fabsf(best
) )
1405 float px
= best
> 0.0f
? rba
->bbx
[0][0]: rba
->bbx
[1][0];
1406 manifold
[0][0] = px
;
1407 manifold
[0][1] = rba
->bbx
[0][1];
1408 manifold
[0][2] = rba
->bbx
[0][2];
1409 manifold
[1][0] = px
;
1410 manifold
[1][1] = rba
->bbx
[1][1];
1411 manifold
[1][2] = rba
->bbx
[0][2];
1412 manifold
[2][0] = px
;
1413 manifold
[2][1] = rba
->bbx
[1][1];
1414 manifold
[2][2] = rba
->bbx
[1][2];
1415 manifold
[3][0] = px
;
1416 manifold
[3][1] = rba
->bbx
[0][1];
1417 manifold
[3][2] = rba
->bbx
[1][2];
1419 else if( axis
== 1 )
1421 float py
= best
> 0.0f
? rba
->bbx
[0][1]: rba
->bbx
[1][1];
1422 manifold
[0][0] = rba
->bbx
[0][0];
1423 manifold
[0][1] = py
;
1424 manifold
[0][2] = rba
->bbx
[0][2];
1425 manifold
[1][0] = rba
->bbx
[1][0];
1426 manifold
[1][1] = py
;
1427 manifold
[1][2] = rba
->bbx
[0][2];
1428 manifold
[2][0] = rba
->bbx
[1][0];
1429 manifold
[2][1] = py
;
1430 manifold
[2][2] = rba
->bbx
[1][2];
1431 manifold
[3][0] = rba
->bbx
[0][0];
1432 manifold
[3][1] = py
;
1433 manifold
[3][2] = rba
->bbx
[1][2];
1437 float pz
= best
> 0.0f
? rba
->bbx
[0][2]: rba
->bbx
[1][2];
1438 manifold
[0][0] = rba
->bbx
[0][0];
1439 manifold
[0][1] = rba
->bbx
[0][1];
1440 manifold
[0][2] = pz
;
1441 manifold
[1][0] = rba
->bbx
[1][0];
1442 manifold
[1][1] = rba
->bbx
[0][1];
1443 manifold
[1][2] = pz
;
1444 manifold
[2][0] = rba
->bbx
[1][0];
1445 manifold
[2][1] = rba
->bbx
[1][1];
1446 manifold
[2][2] = pz
;
1447 manifold
[3][0] = rba
->bbx
[0][0];
1448 manifold
[3][1] = rba
->bbx
[1][1];
1449 manifold
[3][2] = pz
;
1452 for( int j
=0; j
<4; j
++ )
1453 m4x3_mulv( rba
->to_world
, manifold
[j
], manifold
[j
] );
1455 vg_line( manifold
[0], manifold
[1], 0xffffffff );
1456 vg_line( manifold
[1], manifold
[2], 0xffffffff );
1457 vg_line( manifold
[2], manifold
[3], 0xffffffff );
1458 vg_line( manifold
[3], manifold
[0], 0xffffffff );
1460 for( int j
=0; j
<4; j
++ )
1462 rb_ct
*ct
= buf
+count
;
1464 v3_copy( manifold
[j
], ct
->co
);
1465 v3_copy( n
, ct
->n
);
1467 float l0
= v3_dot( tri
[0], n
),
1468 l1
= v3_dot( manifold
[j
], n
);
1470 ct
->p
= (l0
-l1
)*0.5f
;
1474 ct
->type
= k_contact_type_default
;
1486 VG_STATIC
int RB_MATRIX_ERROR( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1488 vg_error( "Collision type is unimplemented between types %d and %d\n",
1489 rba
->type
, rbb
->type
);
1494 VG_STATIC
int rb_sphere_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1496 return rb_capsule_sphere( rbb
, rba
, buf
);
1499 VG_STATIC
int rb_box_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1501 return rb_capsule_box( rbb
, rba
, buf
);
1504 VG_STATIC
int rb_box_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1506 return rb_sphere_box( rbb
, rba
, buf
);
1509 VG_STATIC
int rb_scene_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1511 return rb_box_scene( rbb
, rba
, buf
);
1514 VG_STATIC
int (*rb_jump_table
[4][4])( rigidbody
*a
, rigidbody
*b
, rb_ct
*buf
) =
1516 /* box */ /* Sphere */ /* Capsule */ /* Mesh */
1517 { RB_MATRIX_ERROR
, rb_box_sphere
, rb_box_capsule
, rb_box_scene
},
1518 { rb_sphere_box
, rb_sphere_sphere
, rb_sphere_capsule
, rb_sphere_scene
},
1519 { rb_capsule_box
, rb_capsule_sphere
, rb_capsule_capsule
, RB_MATRIX_ERROR
},
1520 { rb_scene_box
, RB_MATRIX_ERROR
, RB_MATRIX_ERROR
, RB_MATRIX_ERROR
}
1523 VG_STATIC
int rb_collide( rigidbody
*rba
, rigidbody
*rbb
)
1525 int (*collider_jump
)(rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1526 = rb_jump_table
[rba
->type
][rbb
->type
];
1529 * 12 is the maximum manifold size we can generate, so we are forced to abort
1530 * potentially checking any more.
1532 if( rb_contact_count
+ 12 > vg_list_size(rb_contact_buffer
) )
1534 vg_warn( "Too many contacts made in global collider buffer (%d of %d\n)",
1535 rb_contact_count
, vg_list_size(rb_contact_buffer
) );
1540 * FUTURE: Replace this with a more dedicated broad phase pass
1542 if( box_overlap( rba
->bbx_world
, rbb
->bbx_world
) )
1544 int count
= collider_jump( rba
, rbb
, rb_contact_buffer
+rb_contact_count
);
1545 rb_contact_count
+= count
;
1553 * -----------------------------------------------------------------------------
1555 * -----------------------------------------------------------------------------
1558 VG_STATIC
void rb_solver_reset(void)
1560 rb_contact_count
= 0;
1563 VG_STATIC rb_ct
*rb_global_ct(void)
1565 return rb_contact_buffer
+ rb_contact_count
;
1569 * Initializing things like tangent vectors
1571 VG_STATIC
void rb_presolve_contacts( rb_ct
*buffer
, int len
)
1573 for( int i
=0; i
<len
; i
++ )
1575 rb_ct
*ct
= &buffer
[i
];
1577 ct
->bias
= -0.2f
* k_rb_rate
* vg_minf( 0.0f
, -ct
->p
+k_penetration_slop
);
1578 rb_tangent_basis( ct
->n
, ct
->t
[0], ct
->t
[1] );
1581 ct
->type
= k_contact_type_default
;
1583 ct
->norm_impulse
= 0.0f
;
1584 ct
->tangent_impulse
[0] = 0.0f
;
1585 ct
->tangent_impulse
[1] = 0.0f
;
1587 v3f ra
, rb
, raCn
, rbCn
, raCt
, rbCt
;
1588 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
1589 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
1590 v3_cross( ra
, ct
->n
, raCn
);
1591 v3_cross( rb
, ct
->n
, rbCn
);
1593 /* orient inverse inertia tensors */
1595 m3x3_mulv( ct
->rba
->iIw
, raCn
, raCnI
);
1596 m3x3_mulv( ct
->rbb
->iIw
, rbCn
, rbCnI
);
1598 ct
->normal_mass
= ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
1599 ct
->normal_mass
+= v3_dot( raCn
, raCnI
);
1600 ct
->normal_mass
+= v3_dot( rbCn
, rbCnI
);
1601 ct
->normal_mass
= 1.0f
/ct
->normal_mass
;
1603 for( int j
=0; j
<2; j
++ )
1606 v3_cross( ct
->t
[j
], ra
, raCt
);
1607 v3_cross( ct
->t
[j
], rb
, rbCt
);
1608 m3x3_mulv( ct
->rba
->iIw
, raCt
, raCtI
);
1609 m3x3_mulv( ct
->rbb
->iIw
, rbCt
, rbCtI
);
1611 ct
->tangent_mass
[j
] = ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
1612 ct
->tangent_mass
[j
] += v3_dot( raCt
, raCtI
);
1613 ct
->tangent_mass
[j
] += v3_dot( rbCt
, rbCtI
);
1614 ct
->tangent_mass
[j
] = 1.0f
/ct
->tangent_mass
[j
];
1617 rb_debug_contact( ct
);
1622 * Creates relative contact velocity vector, and offsets between each body
1624 VG_STATIC
void rb_rcv( rb_ct
*ct
, v3f rv
, v3f da
, v3f db
)
1626 rigidbody
*rba
= ct
->rba
,
1629 v3_sub( ct
->co
, rba
->co
, da
);
1630 v3_sub( ct
->co
, rbb
->co
, db
);
1633 v3_cross( rba
->w
, da
, rva
);
1634 v3_add( rba
->v
, rva
, rva
);
1635 v3_cross( rbb
->w
, db
, rvb
);
1636 v3_add( rbb
->v
, rvb
, rvb
);
1638 v3_sub( rva
, rvb
, rv
);
1642 * Apply impulse to object
1644 VG_STATIC
void rb_linear_impulse( rigidbody
*rb
, v3f delta
, v3f impulse
)
1647 v3_muladds( rb
->v
, impulse
, rb
->inv_mass
, rb
->v
);
1649 /* Angular velocity */
1651 v3_cross( delta
, impulse
, wa
);
1653 m3x3_mulv( rb
->iIw
, wa
, wa
);
1654 v3_add( rb
->w
, wa
, rb
->w
);
1658 * One iteration to solve the contact constraint
1660 VG_STATIC
void rb_solve_contacts( rb_ct
*buf
, int len
)
1662 for( int i
=0; i
<len
; i
++ )
1664 struct contact
*ct
= &buf
[i
];
1666 rigidbody
*rb
= ct
->rba
;
1669 rb_rcv( ct
, rv
, da
, db
);
1672 for( int j
=0; j
<2; j
++ )
1674 float f
= k_friction
* ct
->norm_impulse
,
1675 vt
= v3_dot( rv
, ct
->t
[j
] ),
1676 lambda
= ct
->tangent_mass
[j
] * -vt
;
1678 float temp
= ct
->tangent_impulse
[j
];
1679 ct
->tangent_impulse
[j
] = vg_clampf( temp
+ lambda
, -f
, f
);
1680 lambda
= ct
->tangent_impulse
[j
] - temp
;
1683 v3_muls( ct
->t
[j
], lambda
, impulse
);
1684 rb_linear_impulse( ct
->rba
, da
, impulse
);
1686 v3_muls( ct
->t
[j
], -lambda
, impulse
);
1687 rb_linear_impulse( ct
->rbb
, db
, impulse
);
1691 rb_rcv( ct
, rv
, da
, db
);
1692 float vn
= v3_dot( rv
, ct
->n
),
1693 lambda
= ct
->normal_mass
* (-vn
+ ct
->bias
);
1695 float temp
= ct
->norm_impulse
;
1696 ct
->norm_impulse
= vg_maxf( temp
+ lambda
, 0.0f
);
1697 lambda
= ct
->norm_impulse
- temp
;
1700 v3_muls( ct
->n
, lambda
, impulse
);
1701 rb_linear_impulse( ct
->rba
, da
, impulse
);
1703 v3_muls( ct
->n
, -lambda
, impulse
);
1704 rb_linear_impulse( ct
->rbb
, db
, impulse
);
1709 * -----------------------------------------------------------------------------
1711 * -----------------------------------------------------------------------------
1714 VG_STATIC
void draw_angle_limit( v3f c
, v3f major
, v3f minor
,
1715 float amin
, float amax
, float measured
,
1720 v3_muls( major
, f
, ay
);
1721 v3_muls( minor
, f
, ax
);
1723 for( int x
=0; x
<16; x
++ )
1725 float t0
= (float)x
/ 16.0f
,
1726 t1
= (float)(x
+1) / 16.0f
,
1727 a0
= vg_lerpf( amin
, amax
, t0
),
1728 a1
= vg_lerpf( amin
, amax
, t1
);
1731 v3_muladds( c
, ay
, cosf(a0
), p0
);
1732 v3_muladds( p0
, ax
, sinf(a0
), p0
);
1733 v3_muladds( c
, ay
, cosf(a1
), p1
);
1734 v3_muladds( p1
, ax
, sinf(a1
), p1
);
1736 vg_line( p0
, p1
, colour
);
1739 vg_line( c
, p0
, colour
);
1741 vg_line( c
, p1
, colour
);
1745 v3_muladds( c
, ay
, cosf(measured
)*1.2f
, p2
);
1746 v3_muladds( p2
, ax
, sinf(measured
)*1.2f
, p2
);
1747 vg_line( c
, p2
, colour
);
1750 VG_STATIC
void rb_debug_constraint_limits( rigidbody
*ra
, rigidbody
*rb
, v3f lca
,
1753 v3f ax
, ay
, az
, bx
, by
, bz
;
1754 m3x3_mulv( ra
->to_world
, (v3f
){1.0f
,0.0f
,0.0f
}, ax
);
1755 m3x3_mulv( ra
->to_world
, (v3f
){0.0f
,1.0f
,0.0f
}, ay
);
1756 m3x3_mulv( ra
->to_world
, (v3f
){0.0f
,0.0f
,1.0f
}, az
);
1757 m3x3_mulv( rb
->to_world
, (v3f
){1.0f
,0.0f
,0.0f
}, bx
);
1758 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,1.0f
,0.0f
}, by
);
1759 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,0.0f
,1.0f
}, bz
);
1762 px
[0] = v3_dot( ay
, by
);
1763 px
[1] = v3_dot( az
, by
);
1765 py
[0] = v3_dot( az
, bz
);
1766 py
[1] = v3_dot( ax
, bz
);
1768 pz
[0] = v3_dot( ax
, bx
);
1769 pz
[1] = v3_dot( ay
, bx
);
1771 float r0
= atan2f( px
[1], px
[0] ),
1772 r1
= atan2f( py
[1], py
[0] ),
1773 r2
= atan2f( pz
[1], pz
[0] );
1776 m4x3_mulv( ra
->to_world
, lca
, c
);
1777 draw_angle_limit( c
, ay
, az
, limits
[0][0], limits
[1][0], r0
, 0xff0000ff );
1778 draw_angle_limit( c
, az
, ax
, limits
[0][1], limits
[1][1], r1
, 0xff00ff00 );
1779 draw_angle_limit( c
, ax
, ay
, limits
[0][2], limits
[1][2], r2
, 0xffff0000 );
1782 VG_STATIC
void rb_limit_cure( rigidbody
*ra
, rigidbody
*rb
, v3f axis
, float d
)
1786 float avx
= v3_dot( ra
->w
, axis
) - v3_dot( rb
->w
, axis
);
1787 float joint_mass
= rb
->inv_mass
+ ra
->inv_mass
;
1788 joint_mass
= 1.0f
/joint_mass
;
1790 float bias
= (k_limit_bias
* k_rb_rate
) * d
,
1791 lambda
= -(avx
+ bias
) * joint_mass
;
1793 /* Angular velocity */
1795 v3_muls( axis
, lambda
* ra
->inv_mass
, wa
);
1796 v3_muls( axis
, -lambda
* rb
->inv_mass
, wb
);
1798 v3_add( ra
->w
, wa
, ra
->w
);
1799 v3_add( rb
->w
, wb
, rb
->w
);
1803 VG_STATIC
void rb_constraint_limits( rigidbody
*ra
, v3f lca
,
1804 rigidbody
*rb
, v3f lcb
, v3f limits
[2] )
1806 v3f ax
, ay
, az
, bx
, by
, bz
;
1807 m3x3_mulv( ra
->to_world
, (v3f
){1.0f
,0.0f
,0.0f
}, ax
);
1808 m3x3_mulv( ra
->to_world
, (v3f
){0.0f
,1.0f
,0.0f
}, ay
);
1809 m3x3_mulv( ra
->to_world
, (v3f
){0.0f
,0.0f
,1.0f
}, az
);
1810 m3x3_mulv( rb
->to_world
, (v3f
){1.0f
,0.0f
,0.0f
}, bx
);
1811 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,1.0f
,0.0f
}, by
);
1812 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,0.0f
,1.0f
}, bz
);
1815 px
[0] = v3_dot( ay
, by
);
1816 px
[1] = v3_dot( az
, by
);
1818 py
[0] = v3_dot( az
, bz
);
1819 py
[1] = v3_dot( ax
, bz
);
1821 pz
[0] = v3_dot( ax
, bx
);
1822 pz
[1] = v3_dot( ay
, bx
);
1824 float r0
= atan2f( px
[1], px
[0] ),
1825 r1
= atan2f( py
[1], py
[0] ),
1826 r2
= atan2f( pz
[1], pz
[0] );
1828 /* calculate angle deltas */
1829 float dx
= 0.0f
, dy
= 0.0f
, dz
= 0.0f
;
1831 if( r0
< limits
[0][0] ) dx
= limits
[0][0] - r0
;
1832 if( r0
> limits
[1][0] ) dx
= limits
[1][0] - r0
;
1833 if( r1
< limits
[0][1] ) dy
= limits
[0][1] - r1
;
1834 if( r1
> limits
[1][1] ) dy
= limits
[1][1] - r1
;
1835 if( r2
< limits
[0][2] ) dz
= limits
[0][2] - r2
;
1836 if( r2
> limits
[1][2] ) dz
= limits
[1][2] - r2
;
1839 m3x3_mulv( ra
->to_world
, lca
, wca
);
1840 m3x3_mulv( rb
->to_world
, lcb
, wcb
);
1842 rb_limit_cure( ra
, rb
, ax
, dx
);
1843 rb_limit_cure( ra
, rb
, ay
, dy
);
1844 rb_limit_cure( ra
, rb
, az
, dz
);
1847 VG_STATIC
void rb_debug_constraint_position( rigidbody
*ra
, v3f lca
,
1848 rigidbody
*rb
, v3f lcb
)
1851 m3x3_mulv( ra
->to_world
, lca
, wca
);
1852 m3x3_mulv( rb
->to_world
, lcb
, wcb
);
1855 v3_add( wca
, ra
->co
, p0
);
1856 v3_add( wcb
, rb
->co
, p1
);
1857 vg_line_pt3( p0
, 0.005f
, 0xffffff00 );
1858 vg_line_pt3( p1
, 0.005f
, 0xffffff00 );
1859 vg_line( p0
, p1
, 0xffffff00 );
1862 VG_STATIC
void rb_constraint_position( rigidbody
*ra
, v3f lca
,
1863 rigidbody
*rb
, v3f lcb
)
1865 /* C = (COa + Ra*LCa) - (COb + Rb*LCb) = 0 */
1867 m3x3_mulv( ra
->to_world
, lca
, wca
);
1868 m3x3_mulv( rb
->to_world
, lcb
, wcb
);
1871 v3_sub( ra
->v
, rb
->v
, rcv
);
1874 v3_cross( ra
->w
, wca
, rcv_Ra
);
1875 v3_cross( rb
->w
, wcb
, rcv_Rb
);
1876 v3_add( rcv_Ra
, rcv
, rcv
);
1877 v3_sub( rcv
, rcv_Rb
, rcv
);
1881 v3_add( wca
, ra
->co
, p0
);
1882 v3_add( wcb
, rb
->co
, p1
);
1883 v3_sub( p1
, p0
, delta
);
1885 float dist2
= v3_length2( delta
);
1887 if( dist2
> 0.00001f
)
1889 float dist
= sqrtf(dist2
);
1890 v3_muls( delta
, 1.0f
/dist
, delta
);
1892 float joint_mass
= rb
->inv_mass
+ ra
->inv_mass
;
1894 v3f raCn
, rbCn
, raCt
, rbCt
;
1895 v3_cross( wca
, delta
, raCn
);
1896 v3_cross( wcb
, delta
, rbCn
);
1898 /* orient inverse inertia tensors */
1900 m3x3_mulv( ra
->iIw
, raCn
, raCnI
);
1901 m3x3_mulv( rb
->iIw
, rbCn
, rbCnI
);
1902 joint_mass
+= v3_dot( raCn
, raCnI
);
1903 joint_mass
+= v3_dot( rbCn
, rbCnI
);
1904 joint_mass
= 1.0f
/joint_mass
;
1906 float vd
= v3_dot( rcv
, delta
),
1907 bias
= -(k_joint_bias
* k_rb_rate
) * dist
,
1908 lambda
= -(vd
+ bias
) * joint_mass
;
1911 v3_muls( delta
, lambda
, impulse
);
1912 rb_linear_impulse( ra
, wca
, impulse
);
1913 v3_muls( delta
, -lambda
, impulse
);
1914 rb_linear_impulse( rb
, wcb
, impulse
);
1917 v3_muladds( ra
->co
, delta
, dist
* k_joint_correction
, ra
->co
);
1918 v3_muladds( rb
->co
, delta
, -dist
* k_joint_correction
, rb
->co
);
1926 VG_STATIC
void rb_effect_simple_bouyency( rigidbody
*ra
, v4f plane
,
1927 float amt
, float drag
)
1930 float depth
= v3_dot( plane
, ra
->co
) - plane
[3],
1931 lambda
= vg_clampf( -depth
, 0.0f
, 1.0f
) * amt
;
1933 v3_muladds( ra
->v
, plane
, lambda
* k_rb_delta
, ra
->v
);
1936 v3_muls( ra
->v
, 1.0f
-(drag
*k_rb_delta
), ra
->v
);
1940 * -----------------------------------------------------------------------------
1941 * BVH implementation, this is ONLY for VG_STATIC rigidbodies, its to slow for
1943 * -----------------------------------------------------------------------------
1946 VG_STATIC
void rb_bh_expand_bound( void *user
, boxf bound
, u32 item_index
)
1948 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
1949 box_concat( bound
, rb
->bbx_world
);
1952 VG_STATIC
float rb_bh_centroid( void *user
, u32 item_index
, int axis
)
1954 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
1955 return (rb
->bbx_world
[axis
][0] + rb
->bbx_world
[1][axis
]) * 0.5f
;
1958 VG_STATIC
void rb_bh_swap( void *user
, u32 ia
, u32 ib
)
1960 rigidbody temp
, *rba
, *rbb
;
1961 rba
= &((rigidbody
*)user
)[ ia
];
1962 rbb
= &((rigidbody
*)user
)[ ib
];
1969 VG_STATIC
void rb_bh_debug( void *user
, u32 item_index
)
1971 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
1972 rb_debug( rb
, 0xff00ffff );
1975 VG_STATIC bh_system bh_system_rigidbodies
=
1977 .expand_bound
= rb_bh_expand_bound
,
1978 .item_centroid
= rb_bh_centroid
,
1979 .item_swap
= rb_bh_swap
,
1980 .item_debug
= rb_bh_debug
,
1984 #endif /* RIGIDBODY_H */