2 * Resources: Box2D - Erin Catto
10 static void rb_tangent_basis( v3f n
, v3f tx
, v3f ty
);
11 static bh_system bh_system_rigidbodies
;
17 #define k_rb_rate 60.0f
18 #define k_rb_delta (1.0f/k_rb_rate)
20 typedef struct rigidbody rigidbody
;
21 typedef struct contact rb_ct
;
31 k_rb_shape_sphere
= 1,
32 k_rb_shape_capsule
= 2,
59 v3f right
, up
, forward
;
66 /* inertia model and inverse world tensor */
70 m4x3f to_world
, to_local
;
75 * Impulses on static objects get re-routed here
77 static rigidbody rb_static_null
=
80 .q
={0.0f
,0.0f
,0.0f
,1.0f
},
88 static void rb_debug( rigidbody
*rb
, u32 colour
);
95 float p
, bias
, norm_impulse
, tangent_impulse
[2],
96 normal_mass
, tangent_mass
[2];
100 rb_contact_buffer
[256];
101 static int rb_contact_count
= 0;
103 static void rb_update_bounds( rigidbody
*rb
)
105 box_copy( rb
->bbx
, rb
->bbx_world
);
106 m4x3_transform_aabb( rb
->to_world
, rb
->bbx_world
);
109 static void rb_update_transform( rigidbody
*rb
)
111 q_normalize( rb
->q
);
112 q_m3x3( rb
->q
, rb
->to_world
);
113 v3_copy( rb
->co
, rb
->to_world
[3] );
115 m4x3_invert_affine( rb
->to_world
, rb
->to_local
);
117 m3x3_mulv( rb
->to_world
, (v3f
){1.0f
,0.0f
, 0.0f
}, rb
->right
);
118 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,1.0f
, 0.0f
}, rb
->up
);
119 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,0.0f
,-1.0f
}, rb
->forward
);
121 m3x3_mul( rb
->iI
, rb
->to_local
, rb
->iIw
);
122 m3x3_mul( rb
->to_world
, rb
->iIw
, rb
->iIw
);
124 rb_update_bounds( rb
);
127 static float sphere_volume( float radius
)
129 float r3
= radius
*radius
*radius
;
130 return (4.0f
/3.0f
) * VG_PIf
* r3
;
133 static void rb_init( rigidbody
*rb
)
137 if( rb
->type
== k_rb_shape_box
)
140 v3_sub( rb
->bbx
[1], rb
->bbx
[0], dims
);
141 volume
= dims
[0]*dims
[1]*dims
[2];
144 vg_info( "Box volume: %f\n", volume
);
146 else if( rb
->type
== k_rb_shape_sphere
)
148 volume
= sphere_volume( rb
->inf
.sphere
.radius
);
149 v3_fill( rb
->bbx
[0], -rb
->inf
.sphere
.radius
);
150 v3_fill( rb
->bbx
[1], rb
->inf
.sphere
.radius
);
152 vg_info( "Sphere volume: %f\n", volume
);
154 else if( rb
->type
== k_rb_shape_capsule
)
156 float r
= rb
->inf
.capsule
.radius
,
157 h
= rb
->inf
.capsule
.height
;
158 volume
= sphere_volume( r
) + VG_PIf
* r
*r
* (h
- r
*2.0f
);
160 v3_fill( rb
->bbx
[0], -rb
->inf
.sphere
.radius
);
161 v3_fill( rb
->bbx
[1], rb
->inf
.sphere
.radius
);
165 else if( rb
->type
== k_rb_shape_scene
)
168 box_copy( rb
->inf
.scene
.pscene
->bbx
, rb
->bbx
);
179 float mass
= 2.0f
*volume
;
180 rb
->inv_mass
= 1.0f
/mass
;
183 v3_sub( rb
->bbx
[1], rb
->bbx
[0], extent
);
184 v3_muls( extent
, 0.5f
, extent
);
186 /* local intertia tensor */
188 float ex2
= scale
*extent
[0]*extent
[0],
189 ey2
= scale
*extent
[1]*extent
[1],
190 ez2
= scale
*extent
[2]*extent
[2];
192 rb
->I
[0] = ((1.0f
/12.0f
) * mass
* (ey2
+ez2
));
193 rb
->I
[1] = ((1.0f
/12.0f
) * mass
* (ex2
+ez2
));
194 rb
->I
[2] = ((1.0f
/12.0f
) * mass
* (ex2
+ey2
));
196 m3x3_identity( rb
->iI
);
197 rb
->iI
[0][0] = rb
->I
[0];
198 rb
->iI
[1][1] = rb
->I
[1];
199 rb
->iI
[2][2] = rb
->I
[2];
200 m3x3_inv( rb
->iI
, rb
->iI
);
206 rb_update_transform( rb
);
209 static void rb_iter( rigidbody
*rb
)
211 v3f gravity
= { 0.0f
, -9.8f
, 0.0f
};
212 v3_muladds( rb
->v
, gravity
, k_rb_delta
, rb
->v
);
214 /* intergrate velocity */
215 v3_muladds( rb
->co
, rb
->v
, k_rb_delta
, rb
->co
);
216 v3_lerp( rb
->w
, (v3f
){0.0f
,0.0f
,0.0f
}, 0.0025f
, rb
->w
);
218 /* inegrate inertia */
219 if( v3_length2( rb
->w
) > 0.0f
)
223 v3_copy( rb
->w
, axis
);
225 float mag
= v3_length( axis
);
226 v3_divs( axis
, mag
, axis
);
227 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
);
228 q_mul( rotation
, rb
->q
, rb
->q
);
232 static void rb_torque( rigidbody
*rb
, v3f axis
, float mag
)
234 v3_muladds( rb
->w
, axis
, mag
*k_rb_delta
, rb
->w
);
237 static void rb_tangent_basis( v3f n
, v3f tx
, v3f ty
)
239 /* Compute tangent basis (box2d) */
240 if( fabsf( n
[0] ) >= 0.57735027f
)
254 v3_cross( n
, tx
, ty
);
257 static void rb_solver_reset(void);
259 static void rb_build_manifold_terrain( rigidbody
*rb
);
260 static void rb_build_manifold_terrain_sphere( rigidbody
*rb
);
262 static void rb_solve_contacts( rb_ct
*buf
, int len
);
263 static void rb_presolve_contacts( rb_ct
*buffer
, int len
);
266 * These closest point tests were learned from Real-Time Collision Detection by
269 static float closest_segment_segment( v3f p1
, v3f q1
, v3f p2
, v3f q2
,
270 float *s
, float *t
, v3f c1
, v3f c2
)
273 v3_sub( q1
, p1
, d1
);
274 v3_sub( q2
, p2
, d2
);
277 float a
= v3_length2( d1
),
278 e
= v3_length2( d2
),
281 const float kEpsilon
= 0.0001f
;
283 if( a
<= kEpsilon
&& e
<= kEpsilon
)
291 v3_sub( c1
, c2
, v0
);
293 return v3_length2( v0
);
299 *t
= vg_clampf( f
/ e
, 0.0f
, 1.0f
);
303 float c
= v3_dot( d1
, r
);
307 *s
= vg_clampf( -c
/ a
, 0.0f
, 1.0f
);
311 float b
= v3_dot(d1
,d2
),
316 *s
= vg_clampf((b
*f
- c
*e
)/d
, 0.0f
, 1.0f
);
328 *s
= vg_clampf( -c
/ a
, 0.0f
, 1.0f
);
333 *s
= vg_clampf((b
-c
)/a
,0.0f
,1.0f
);
338 v3_muladds( p1
, d1
, *s
, c1
);
339 v3_muladds( p2
, d2
, *t
, c2
);
342 v3_sub( c1
, c2
, v0
);
343 return v3_length2( v0
);
346 static void closest_point_aabb( v3f p
, boxf box
, v3f dest
)
348 v3_maxv( p
, box
[0], dest
);
349 v3_minv( dest
, box
[1], dest
);
352 static void closest_point_obb( v3f p
, rigidbody
*rb
, v3f dest
)
355 m4x3_mulv( rb
->to_local
, p
, local
);
356 closest_point_aabb( local
, rb
->bbx
, local
);
357 m4x3_mulv( rb
->to_world
, local
, dest
);
360 static float closest_point_segment( v3f a
, v3f b
, v3f point
, v3f dest
)
364 v3_sub( point
, a
, v1
);
366 float t
= v3_dot( v1
, v0
) / v3_length2(v0
);
367 t
= vg_clampf(t
,0.0f
,1.0f
);
368 v3_muladds( a
, v0
, t
, dest
);
372 static void closest_on_triangle( v3f p
, v3f tri
[3], v3f dest
)
377 /* Region outside A */
378 v3_sub( tri
[1], tri
[0], ab
);
379 v3_sub( tri
[2], tri
[0], ac
);
380 v3_sub( p
, tri
[0], ap
);
384 if( d1
<= 0.0f
&& d2
<= 0.0f
)
386 v3_copy( tri
[0], dest
);
387 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
391 /* Region outside B */
395 v3_sub( p
, tri
[1], bp
);
396 d3
= v3_dot( ab
, bp
);
397 d4
= v3_dot( ac
, bp
);
399 if( d3
>= 0.0f
&& d4
<= d3
)
401 v3_copy( tri
[1], dest
);
402 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
406 /* Edge region of AB */
407 float vc
= d1
*d4
- d3
*d2
;
408 if( vc
<= 0.0f
&& d1
>= 0.0f
&& d3
<= 0.0f
)
410 float v
= d1
/ (d1
-d3
);
411 v3_muladds( tri
[0], ab
, v
, dest
);
412 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
416 /* Region outside C */
419 v3_sub( p
, tri
[2], cp
);
423 if( d6
>= 0.0f
&& d5
<= d6
)
425 v3_copy( tri
[2], dest
);
426 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
431 float vb
= d5
*d2
- d1
*d6
;
432 if( vb
<= 0.0f
&& d2
>= 0.0f
&& d6
<= 0.0f
)
434 float w
= d2
/ (d2
-d6
);
435 v3_muladds( tri
[0], ac
, w
, dest
);
436 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
441 float va
= d3
*d6
- d5
*d4
;
442 if( va
<= 0.0f
&& (d4
-d3
) >= 0.0f
&& (d5
-d6
) >= 0.0f
)
444 float w
= (d4
-d3
) / ((d4
-d3
) + (d5
-d6
));
446 v3_sub( tri
[2], tri
[1], bc
);
447 v3_muladds( tri
[1], bc
, w
, dest
);
448 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
452 /* P inside region, Q via barycentric coordinates uvw */
453 float d
= 1.0f
/(va
+vb
+vc
),
457 v3_muladds( tri
[0], ab
, v
, dest
);
458 v3_muladds( dest
, ac
, w
, dest
);
462 static void closest_on_triangle_1( v3f p
, v3f tri
[3], v3f dest
)
467 /* Region outside A */
468 v3_sub( tri
[1], tri
[0], ab
);
469 v3_sub( tri
[2], tri
[0], ac
);
470 v3_sub( p
, tri
[0], ap
);
474 if( d1
<= 0.0f
&& d2
<= 0.0f
)
476 v3_copy( tri
[0], dest
);
480 /* Region outside B */
484 v3_sub( p
, tri
[1], bp
);
485 d3
= v3_dot( ab
, bp
);
486 d4
= v3_dot( ac
, bp
);
488 if( d3
>= 0.0f
&& d4
<= d3
)
490 v3_copy( tri
[1], dest
);
494 /* Edge region of AB */
495 float vc
= d1
*d4
- d3
*d2
;
496 if( vc
<= 0.0f
&& d1
>= 0.0f
&& d3
<= 0.0f
)
498 float v
= d1
/ (d1
-d3
);
499 v3_muladds( tri
[0], ab
, v
, dest
);
503 /* Region outside C */
506 v3_sub( p
, tri
[2], cp
);
510 if( d6
>= 0.0f
&& d5
<= d6
)
512 v3_copy( tri
[2], dest
);
517 float vb
= d5
*d2
- d1
*d6
;
518 if( vb
<= 0.0f
&& d2
>= 0.0f
&& d6
<= 0.0f
)
520 float w
= d2
/ (d2
-d6
);
521 v3_muladds( tri
[0], ac
, w
, dest
);
526 float va
= d3
*d6
- d5
*d4
;
527 if( va
<= 0.0f
&& (d4
-d3
) >= 0.0f
&& (d5
-d6
) >= 0.0f
)
529 float w
= (d4
-d3
) / ((d4
-d3
) + (d5
-d6
));
531 v3_sub( tri
[2], tri
[1], bc
);
532 v3_muladds( tri
[1], bc
, w
, dest
);
536 /* P inside region, Q via barycentric coordinates uvw */
537 float d
= 1.0f
/(va
+vb
+vc
),
541 v3_muladds( tri
[0], ab
, v
, dest
);
542 v3_muladds( dest
, ac
, w
, dest
);
545 static int rb_intersect_planes( v4f p0
, v4f p1
, v4f p2
, v3f p
)
548 v3_cross( p1
, p2
, u
);
549 float d
= v3_dot( p0
, u
);
551 if( fabsf(d
) < 0.0001f
)
554 v3_muls( u
, p0
[3], p
);
557 v3_muls( p1
, p2
[3], v0
);
558 v3_muladds( v0
, p2
, -p1
[3], v0
);
559 v3_cross( p0
, v0
, v1
);
561 v3_muls( p
, 1.0f
/d
, p
);
566 int rb_intersect_planes_1( v4f a
, v4f b
, v4f c
, v3f p
)
568 float const epsilon
= 0.001;
576 if( d
< epsilon
&& d
> -epsilon
) return 0;
582 v3_muls( bc
, -a
[3], p
);
583 v3_muladds( p
, ca
, -b
[3], p
);
584 v3_muladds( p
, ab
, -c
[3], p
);
594 * These do not automatically allocate contacts, an appropriately sized
595 * buffer must be supplied. The function returns the size of the manifold
596 * which was generated.
598 * The values set on the contacts are: n, co, p, rba, rbb
601 static void rb_debug_contact( rb_ct
*ct
)
604 v3_muladds( ct
->co
, ct
->n
, 0.1f
, p1
);
605 vg_line_pt3( ct
->co
, 0.025f
, 0xff0000ff );
606 vg_line( ct
->co
, p1
, 0xffffffff );
610 * By collecting the minimum(time) and maximum(time) pairs of points, we
611 * build a reduced and stable exact manifold.
614 * rx: minimum distance of these points
615 * dx: the delta between the two points
617 * pairs will only ammend these if they are creating a collision
619 typedef struct capsule_manifold capsule_manifold
;
620 struct capsule_manifold
628 * Expand a line manifold with a new pair. t value is the time along segment
629 * on the oriented object which created this pair.
631 static void rb_capsule_manifold( v3f pa
, v3f pb
, float t
, float r
,
632 capsule_manifold
*manifold
)
635 v3_sub( pa
, pb
, delta
);
637 if( v3_length2(delta
) < r
*r
)
639 if( t
< manifold
->t0
)
641 v3_copy( delta
, manifold
->d0
);
646 if( t
> manifold
->t1
)
648 v3_copy( delta
, manifold
->d1
);
655 static void rb_capsule_manifold_init( capsule_manifold
*manifold
)
657 manifold
->t0
= INFINITY
;
658 manifold
->t1
= -INFINITY
;
661 static int rb_capsule_manifold_done( rigidbody
*rba
, rigidbody
*rbb
,
662 capsule_manifold
*manifold
, rb_ct
*buf
)
664 float h
= rba
->inf
.capsule
.height
,
665 ra
= rba
->inf
.capsule
.radius
;
668 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+ra
, p0
);
669 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-ra
, p1
);
672 if( manifold
->t0
<= 1.0f
)
677 v3_muls( p0
, 1.0f
-manifold
->t0
, pa
);
678 v3_muladds( pa
, p1
, manifold
->t0
, pa
);
680 float d
= v3_length( manifold
->d0
);
681 v3_muls( manifold
->d0
, 1.0f
/d
, ct
->n
);
682 v3_muladds( pa
, ct
->n
, -ra
, ct
->co
);
684 ct
->p
= manifold
->r0
- d
;
691 if( (manifold
->t1
>= 0.0f
) && (manifold
->t0
!= manifold
->t1
) )
693 rb_ct
*ct
= buf
+count
;
696 v3_muls( p0
, 1.0f
-manifold
->t1
, pa
);
697 v3_muladds( pa
, p1
, manifold
->t1
, pa
);
699 float d
= v3_length( manifold
->d1
);
700 v3_muls( manifold
->d1
, 1.0f
/d
, ct
->n
);
701 v3_muladds( pa
, ct
->n
, -ra
, ct
->co
);
703 ct
->p
= manifold
->r1
- d
;
715 vg_line( buf
[0].co
, buf
[1].co
, 0xff0000ff );
720 static int rb_capsule_vs_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
722 float h
= rba
->inf
.capsule
.height
,
723 ra
= rba
->inf
.capsule
.radius
,
724 rb
= rbb
->inf
.sphere
.radius
;
727 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+ra
, p0
);
728 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-ra
, p1
);
731 closest_point_segment( p0
, p1
, rbb
->co
, c
);
732 v3_sub( c
, rbb
->co
, delta
);
734 float d2
= v3_length2(delta
),
742 v3_muls( delta
, 1.0f
/d
, ct
->n
);
746 v3_muladds( c
, ct
->n
, -ra
, p0
);
747 v3_muladds( rbb
->co
, ct
->n
, rb
, p1
);
748 v3_add( p0
, p1
, ct
->co
);
749 v3_muls( ct
->co
, 0.5f
, ct
->co
);
760 static int rb_capsule_vs_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
762 float ha
= rba
->inf
.capsule
.height
,
763 hb
= rbb
->inf
.capsule
.height
,
764 ra
= rba
->inf
.capsule
.radius
,
765 rb
= rbb
->inf
.capsule
.radius
,
769 v3_muladds( rba
->co
, rba
->up
, -ha
*0.5f
+ra
, p0
);
770 v3_muladds( rba
->co
, rba
->up
, ha
*0.5f
-ra
, p1
);
771 v3_muladds( rbb
->co
, rbb
->up
, -hb
*0.5f
+rb
, p2
);
772 v3_muladds( rbb
->co
, rbb
->up
, hb
*0.5f
-rb
, p3
);
774 capsule_manifold manifold
;
775 rb_capsule_manifold_init( &manifold
);
779 closest_segment_segment( p0
, p1
, p2
, p3
, &ta
, &tb
, pa
, pb
);
780 rb_capsule_manifold( pa
, pb
, ta
, r
, &manifold
);
782 ta
= closest_point_segment( p0
, p1
, p2
, pa
);
783 tb
= closest_point_segment( p0
, p1
, p3
, pb
);
784 rb_capsule_manifold( pa
, p2
, ta
, r
, &manifold
);
785 rb_capsule_manifold( pb
, p3
, tb
, r
, &manifold
);
787 closest_point_segment( p2
, p3
, p0
, pa
);
788 closest_point_segment( p2
, p3
, p1
, pb
);
789 rb_capsule_manifold( p0
, pa
, 0.0f
, r
, &manifold
);
790 rb_capsule_manifold( p1
, pb
, 1.0f
, r
, &manifold
);
792 return rb_capsule_manifold_done( rba
, rbb
, &manifold
, buf
);
796 * Generates up to two contacts; optimised for the most stable manifold
798 static int rb_capsule_vs_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
800 float h
= rba
->inf
.capsule
.height
,
801 r
= rba
->inf
.capsule
.radius
;
804 * Solving this in symetric local space of the cube saves us some time and a
805 * couple branches when it comes to the quad stage.
808 v3_add( rbb
->bbx
[0], rbb
->bbx
[1], centroid
);
809 v3_muls( centroid
, 0.5f
, centroid
);
812 v3_sub( rbb
->bbx
[0], centroid
, bbx
[0] );
813 v3_sub( rbb
->bbx
[1], centroid
, bbx
[1] );
815 v3f pc
, p0w
, p1w
, p0
, p1
;
816 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+r
, p0w
);
817 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-r
, p1w
);
819 m4x3_mulv( rbb
->to_local
, p0w
, p0
);
820 m4x3_mulv( rbb
->to_local
, p1w
, p1
);
821 v3_sub( p0
, centroid
, p0
);
822 v3_sub( p1
, centroid
, p1
);
823 v3_add( p0
, p1
, pc
);
824 v3_muls( pc
, 0.5f
, pc
);
827 * Finding an appropriate quad to collide lines with
830 v3_div( pc
, bbx
[1], region
);
833 if( (fabsf(region
[0]) > fabsf(region
[1])) &&
834 (fabsf(region
[0]) > fabsf(region
[2])) )
836 float px
= vg_signf(region
[0]) * bbx
[1][0];
837 v3_copy( (v3f
){ px
, bbx
[0][1], bbx
[0][2] }, quad
[0] );
838 v3_copy( (v3f
){ px
, bbx
[1][1], bbx
[0][2] }, quad
[1] );
839 v3_copy( (v3f
){ px
, bbx
[1][1], bbx
[1][2] }, quad
[2] );
840 v3_copy( (v3f
){ px
, bbx
[0][1], bbx
[1][2] }, quad
[3] );
842 else if( fabsf(region
[1]) > fabsf(region
[2]) )
844 float py
= vg_signf(region
[1]) * bbx
[1][1];
845 v3_copy( (v3f
){ bbx
[0][0], py
, bbx
[0][2] }, quad
[0] );
846 v3_copy( (v3f
){ bbx
[1][0], py
, bbx
[0][2] }, quad
[1] );
847 v3_copy( (v3f
){ bbx
[1][0], py
, bbx
[1][2] }, quad
[2] );
848 v3_copy( (v3f
){ bbx
[0][0], py
, bbx
[1][2] }, quad
[3] );
852 float pz
= vg_signf(region
[2]) * bbx
[1][2];
853 v3_copy( (v3f
){ bbx
[0][0], bbx
[0][1], pz
}, quad
[0] );
854 v3_copy( (v3f
){ bbx
[1][0], bbx
[0][1], pz
}, quad
[1] );
855 v3_copy( (v3f
){ bbx
[1][0], bbx
[1][1], pz
}, quad
[2] );
856 v3_copy( (v3f
){ bbx
[0][0], bbx
[1][1], pz
}, quad
[3] );
859 capsule_manifold manifold
;
860 rb_capsule_manifold_init( &manifold
);
863 closest_point_aabb( p0
, bbx
, c0
);
864 closest_point_aabb( p1
, bbx
, c1
);
867 v3_sub( c0
, p0
, d0
);
868 v3_sub( c1
, p1
, d1
);
869 v3_sub( p1
, p0
, da
);
876 if( v3_dot( da
, d0
) <= 0.01f
)
877 rb_capsule_manifold( p0
, c0
, 0.0f
, r
, &manifold
);
879 if( v3_dot( da
, d1
) >= -0.01f
)
880 rb_capsule_manifold( p1
, c1
, 1.0f
, r
, &manifold
);
882 for( int i
=0; i
<4; i
++ )
889 closest_segment_segment( p0
, p1
, quad
[i0
], quad
[i1
], &ta
, &tb
, ca
, cb
);
890 rb_capsule_manifold( ca
, cb
, ta
, r
, &manifold
);
894 * Create final contacts based on line manifold
896 m3x3_mulv( rbb
->to_world
, manifold
.d0
, manifold
.d0
);
897 m3x3_mulv( rbb
->to_world
, manifold
.d1
, manifold
.d1
);
904 for( int i
=0; i
<4; i
++ )
910 v3_add( quad
[i0
], centroid
, q0
);
911 v3_add( quad
[i1
], centroid
, q1
);
913 m4x3_mulv( rbb
->to_world
, q0
, q0
);
914 m4x3_mulv( rbb
->to_world
, q1
, q1
);
916 vg_line( q0
, q1
, 0xffffffff );
920 return rb_capsule_manifold_done( rba
, rbb
, &manifold
, buf
);
923 static int rb_sphere_vs_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
927 closest_point_obb( rba
->co
, rbb
, co
);
928 v3_sub( rba
->co
, co
, delta
);
930 float d2
= v3_length2(delta
),
931 r
= rba
->inf
.sphere
.radius
;
940 v3_sub( rba
->co
, rbb
->co
, delta
);
943 * some extra testing is required to find the best axis to push the
944 * object back outside the box. Since there isnt a clear seperating
945 * vector already, especially on really high aspect boxes.
947 float lx
= v3_dot( rbb
->right
, delta
),
948 ly
= v3_dot( rbb
->up
, delta
),
949 lz
= v3_dot( rbb
->forward
, delta
),
950 px
= rbb
->bbx
[1][0] - fabsf(lx
),
951 py
= rbb
->bbx
[1][1] - fabsf(ly
),
952 pz
= rbb
->bbx
[1][2] - fabsf(lz
);
954 if( px
< py
&& px
< pz
)
955 v3_muls( rbb
->right
, vg_signf(lx
), ct
->n
);
957 v3_muls( rbb
->up
, vg_signf(ly
), ct
->n
);
959 v3_muls( rbb
->forward
, vg_signf(lz
), ct
->n
);
961 v3_muladds( rba
->co
, ct
->n
, -r
, ct
->co
);
967 v3_muls( delta
, 1.0f
/d
, ct
->n
);
969 v3_copy( co
, ct
->co
);
980 static int rb_sphere_vs_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
983 v3_sub( rba
->co
, rbb
->co
, delta
);
985 float d2
= v3_length2(delta
),
986 r
= rba
->inf
.sphere
.radius
+ rbb
->inf
.sphere
.radius
;
993 v3_muls( delta
, 1.0f
/d
, ct
->n
);
996 v3_muladds( rba
->co
, ct
->n
,-rba
->inf
.sphere
.radius
, p0
);
997 v3_muladds( rbb
->co
, ct
->n
, rbb
->inf
.sphere
.radius
, p1
);
998 v3_add( p0
, p1
, ct
->co
);
999 v3_muls( ct
->co
, 0.5f
, ct
->co
);
1009 /* TODO: these guys */
1011 static int rb_capsule_vs_scene( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1015 int len
= bh_select( &rbb
->inf
.scene
.pscene
->bhtris
,
1016 rba
->bbx_world
, geo
, 128 );
1021 static int rb_sphere_vs_triangle( rigidbody
*rba
, rigidbody
*rbb
,
1022 v3f tri
[3], rb_ct
*buf
)
1026 closest_on_triangle( rba
->co
, tri
, co
);
1027 v3_sub( rba
->co
, co
, delta
);
1029 vg_line( rba
->co
, co
, 0xffff0000 );
1030 vg_line_pt3( rba
->co
, 0.1f
, 0xff00ffff );
1032 float d2
= v3_length2( delta
),
1033 r
= rba
->inf
.sphere
.radius
;
1040 v3_sub( tri
[2], tri
[0], ab
);
1041 v3_sub( tri
[1], tri
[0], ac
);
1042 v3_cross( ac
, ab
, tn
);
1043 v3_copy( tn
, ct
->n
);
1044 v3_normalize( ct
->n
);
1046 float d
= sqrtf(d2
);
1048 v3_copy( co
, ct
->co
);
1058 static int rb_sphere_vs_scene( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1060 scene
*sc
= rbb
->inf
.scene
.pscene
;
1064 int len
= bh_select( &sc
->bhtris
, rba
->bbx_world
, geo
, 128 );
1068 for( int i
=0; i
<len
; i
++ )
1070 u32
*ptri
= &sc
->indices
[ geo
[i
]*3 ];
1072 for( int j
=0; j
<3; j
++ )
1073 v3_copy( sc
->verts
[ptri
[j
]].co
, tri
[j
] );
1075 vg_line(tri
[0],tri
[1],0xff00ff00 );
1076 vg_line(tri
[1],tri
[2],0xff00ff00 );
1077 vg_line(tri
[2],tri
[0],0xff00ff00 );
1079 buf
[count
].element_id
= ptri
[0];
1080 count
+= rb_sphere_vs_triangle( rba
, rbb
, tri
, buf
+count
);
1084 vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
1092 static float rb_box_plane_interval( rigidbody
*rba
, v4f p
)
1094 /* TODO: Make boxes COG aligned as is every other shape.
1095 * or create COG vector.
1096 * TODO: Make forward actually point in the right fucking direction. */
1098 v3_sub( rba
->bbx
[1], rba
->bbx
[0], e
);
1099 v3_muls( e
, 0.5f
, e
);
1100 v3_add( rba
->bbx
[0], e
, c
);
1101 m4x3_mulv( rba
->to_world
, c
, c
);
1104 e
[0]*fabsf( v3_dot(p
, rba
->right
)) +
1105 e
[1]*fabsf( v3_dot(p
, rba
->up
)) +
1106 e
[2]*fabsf(-v3_dot(p
, rba
->forward
)),
1107 s
= v3_dot( p
, c
) - p
[3];
1112 static int rb_box_triangle_interval( v3f extent
, v3f axis
, v3f tri
[3] )
1116 r
= extent
[0] * fabsf(axis
[0]) +
1117 extent
[1] * fabsf(axis
[1]) +
1118 extent
[2] * fabsf(axis
[2]),
1120 p0
= v3_dot( axis
, tri
[0] ),
1121 p1
= v3_dot( axis
, tri
[1] ),
1122 p2
= v3_dot( axis
, tri
[2] ),
1124 e
= vg_maxf(-vg_maxf(p0
,vg_maxf(p1
,p2
)), vg_minf(p0
,vg_minf(p1
,p2
)));
1126 if( e
> r
) return 0;
1130 static int rb_box_triangle_sat( rigidbody
*rba
, v3f tri_src
[3] )
1135 v3_sub( rba
->bbx
[1], rba
->bbx
[0], extent
);
1136 v3_muls( extent
, 0.5f
, extent
);
1137 v3_add( rba
->bbx
[0], extent
, c
);
1139 for( int i
=0; i
<3; i
++ )
1141 m4x3_mulv( rba
->to_local
, tri_src
[i
], tri
[i
] );
1142 v3_sub( tri
[i
], c
, tri
[i
] );
1146 if(!rb_box_triangle_interval( extent
, (v3f
){1.0f
,0.0f
,0.0f
}, tri
)) return 0;
1147 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,1.0f
,0.0f
}, tri
)) return 0;
1148 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,0.0f
,1.0f
}, tri
)) return 0;
1150 v3f v0
,v1
,v2
,n
, e0
,e1
,e2
;
1151 v3_sub( tri
[1], tri
[0], v0
);
1152 v3_sub( tri
[2], tri
[0], v1
);
1153 v3_sub( tri
[2], tri
[1], v2
);
1157 v3_cross( v0
, v1
, n
);
1158 v3_cross( v0
, n
, e0
);
1159 v3_cross( n
, v1
, e1
);
1160 v3_cross( v2
, n
, e2
);
1163 if(!rb_box_triangle_interval( extent
, n
, tri
)) return 0;
1166 v3_cross( e0
, (v3f
){1.0f
,0.0f
,0.0f
}, axis
[0] );
1167 v3_cross( e0
, (v3f
){0.0f
,1.0f
,0.0f
}, axis
[1] );
1168 v3_cross( e0
, (v3f
){0.0f
,0.0f
,1.0f
}, axis
[2] );
1169 v3_cross( e1
, (v3f
){1.0f
,0.0f
,0.0f
}, axis
[3] );
1170 v3_cross( e1
, (v3f
){0.0f
,1.0f
,0.0f
}, axis
[4] );
1171 v3_cross( e1
, (v3f
){0.0f
,0.0f
,1.0f
}, axis
[5] );
1172 v3_cross( e2
, (v3f
){1.0f
,0.0f
,0.0f
}, axis
[6] );
1173 v3_cross( e2
, (v3f
){0.0f
,1.0f
,0.0f
}, axis
[7] );
1174 v3_cross( e2
, (v3f
){0.0f
,0.0f
,1.0f
}, axis
[8] );
1176 for( int i
=0; i
<9; i
++ )
1177 if(!rb_box_triangle_interval( extent
, axis
[i
], tri
)) return 0;
1182 static int rb_box_vs_scene( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1184 scene
*sc
= rbb
->inf
.scene
.pscene
;
1188 int len
= bh_select( &sc
->bhtris
, rba
->bbx_world
, geo
, 128 );
1192 for( int i
=0; i
<len
; i
++ )
1194 u32
*ptri
= &sc
->indices
[ geo
[i
]*3 ];
1196 for( int j
=0; j
<3; j
++ )
1197 v3_copy( sc
->verts
[ptri
[j
]].co
, tri
[j
] );
1199 if( rb_box_triangle_sat( rba
, tri
) )
1201 vg_line(tri
[0],tri
[1],0xff50ff00 );
1202 vg_line(tri
[1],tri
[2],0xff50ff00 );
1203 vg_line(tri
[2],tri
[0],0xff50ff00 );
1207 vg_line(tri
[0],tri
[1],0xff0000ff );
1208 vg_line(tri
[1],tri
[2],0xff0000ff );
1209 vg_line(tri
[2],tri
[0],0xff0000ff );
1215 v3_sub( tri
[1], tri
[0], v0
);
1216 v3_sub( tri
[2], tri
[0], v1
);
1217 v3_cross( v0
, v1
, n
);
1220 /* find best feature */
1221 float best
= v3_dot( rba
->right
, n
);
1224 float cy
= v3_dot( rba
->up
, n
);
1225 if( fabsf(cy
) > fabsf(best
) )
1231 /* TODO: THIS IS WRONG DIRECTION */
1232 float cz
= -v3_dot( rba
->forward
, n
);
1233 if( fabsf(cz
) > fabsf(best
) )
1243 float px
= best
> 0.0f
? rba
->bbx
[0][0]: rba
->bbx
[1][0];
1244 manifold
[0][0] = px
;
1245 manifold
[0][1] = rba
->bbx
[0][1];
1246 manifold
[0][2] = rba
->bbx
[0][2];
1247 manifold
[1][0] = px
;
1248 manifold
[1][1] = rba
->bbx
[1][1];
1249 manifold
[1][2] = rba
->bbx
[0][2];
1250 manifold
[2][0] = px
;
1251 manifold
[2][1] = rba
->bbx
[1][1];
1252 manifold
[2][2] = rba
->bbx
[1][2];
1253 manifold
[3][0] = px
;
1254 manifold
[3][1] = rba
->bbx
[0][1];
1255 manifold
[3][2] = rba
->bbx
[1][2];
1257 else if( axis
== 1 )
1259 float py
= best
> 0.0f
? rba
->bbx
[0][1]: rba
->bbx
[1][1];
1260 manifold
[0][0] = rba
->bbx
[0][0];
1261 manifold
[0][1] = py
;
1262 manifold
[0][2] = rba
->bbx
[0][2];
1263 manifold
[1][0] = rba
->bbx
[1][0];
1264 manifold
[1][1] = py
;
1265 manifold
[1][2] = rba
->bbx
[0][2];
1266 manifold
[2][0] = rba
->bbx
[1][0];
1267 manifold
[2][1] = py
;
1268 manifold
[2][2] = rba
->bbx
[1][2];
1269 manifold
[3][0] = rba
->bbx
[0][0];
1270 manifold
[3][1] = py
;
1271 manifold
[3][2] = rba
->bbx
[1][2];
1275 float pz
= best
> 0.0f
? rba
->bbx
[0][2]: rba
->bbx
[1][2];
1276 manifold
[0][0] = rba
->bbx
[0][0];
1277 manifold
[0][1] = rba
->bbx
[0][1];
1278 manifold
[0][2] = pz
;
1279 manifold
[1][0] = rba
->bbx
[1][0];
1280 manifold
[1][1] = rba
->bbx
[0][1];
1281 manifold
[1][2] = pz
;
1282 manifold
[2][0] = rba
->bbx
[1][0];
1283 manifold
[2][1] = rba
->bbx
[1][1];
1284 manifold
[2][2] = pz
;
1285 manifold
[3][0] = rba
->bbx
[0][0];
1286 manifold
[3][1] = rba
->bbx
[1][1];
1287 manifold
[3][2] = pz
;
1290 for( int j
=0; j
<4; j
++ )
1291 m4x3_mulv( rba
->to_world
, manifold
[j
], manifold
[j
] );
1293 vg_line( manifold
[0], manifold
[1], 0xffffffff );
1294 vg_line( manifold
[1], manifold
[2], 0xffffffff );
1295 vg_line( manifold
[2], manifold
[3], 0xffffffff );
1296 vg_line( manifold
[3], manifold
[0], 0xffffffff );
1298 for( int j
=0; j
<4; j
++ )
1300 rb_ct
*ct
= buf
+count
;
1302 v3_copy( manifold
[j
], ct
->co
);
1303 v3_copy( n
, ct
->n
);
1305 float l0
= v3_dot( tri
[0], n
),
1306 l1
= v3_dot( manifold
[j
], n
);
1308 ct
->p
= (l0
-l1
)*0.5f
;
1323 static int RB_MATRIX_ERROR( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1325 vg_error( "Collision type is unimplemented between types %d and %d\n",
1326 rba
->type
, rbb
->type
);
1331 static int rb_sphere_vs_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1333 return rb_capsule_vs_sphere( rbb
, rba
, buf
);
1336 static int rb_box_vs_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1338 return rb_capsule_vs_box( rbb
, rba
, buf
);
1341 static int rb_box_vs_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1343 return rb_sphere_vs_box( rbb
, rba
, buf
);
1346 static int rb_scene_vs_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1348 return rb_box_vs_scene( rbb
, rba
, buf
);
1351 static int (*rb_jump_table
[4][4])( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)=
1352 { /* box */ /* Sphere */ /* Capsule */ /* Mesh */
1353 /*box */ { RB_MATRIX_ERROR
, rb_box_vs_sphere
, rb_box_vs_capsule
, rb_box_vs_scene
},
1354 /*sphere */ { rb_sphere_vs_box
, rb_sphere_vs_sphere
, rb_sphere_vs_capsule
, rb_sphere_vs_scene
},
1355 /*capsule*/ { rb_capsule_vs_box
,rb_capsule_vs_sphere
,rb_capsule_vs_capsule
,RB_MATRIX_ERROR
},
1356 /*mesh */ { rb_scene_vs_box
, RB_MATRIX_ERROR
, RB_MATRIX_ERROR
, RB_MATRIX_ERROR
}
1359 static int rb_collide( rigidbody
*rba
, rigidbody
*rbb
)
1361 int (*collider_jump
)(rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1362 = rb_jump_table
[rba
->type
][rbb
->type
];
1365 * 12 is the maximum manifold size we can generate, so we are forced to abort
1366 * potentially checking any more.
1368 if( rb_contact_count
+ 12 > vg_list_size(rb_contact_buffer
) )
1370 vg_warn( "Too many contacts made in global collider buffer (%d of %d\n)",
1371 rb_contact_count
, vg_list_size(rb_contact_buffer
) );
1376 * TODO: Replace this with a more dedicated broad phase pass
1378 if( box_overlap( rba
->bbx_world
, rbb
->bbx_world
) )
1380 int count
= collider_jump( rba
, rbb
, rb_contact_buffer
+rb_contact_count
);
1381 rb_contact_count
+= count
;
1394 * This function does not accept triangle as a dynamic object, it is assumed
1395 * to always be static.
1397 * The triangle is also assumed to be one sided for better detection
1399 static int rb_sphere_vs_triangle( rigidbody
*rba
, v3f tri
[3], rb_ct
*buf
)
1403 closest_on_triangle( rba
->co
, tri
, co
);
1404 v3_sub( rba
->co
, co
, delta
);
1406 float d2
= v3_length2( delta
),
1407 r
= rba
->inf
.sphere
.radius
;
1412 v3_sub( tri
[1], tri
[0], ab
);
1413 v3_sub( tri
[2], tri
[0], ac
);
1414 v3_cross( ac
, ab
, tn
);
1416 if( v3_dot( delta
, tn
) > 0.0f
)
1417 v3_muls( delta
, -1.0f
, delta
);
1419 float d
= sqrtf(d2
);
1422 v3_muls( delta
, 1.0f
/d
, ct
->n
);
1423 v3_copy( co
, ct
->co
);
1426 ct
->rbb
= &rb_static_null
;
1433 static int sphere_vs_triangle( v3f c
, float r
, v3f tri
[3],
1434 v3f co
, v3f norm
, float *p
)
1437 closest_on_triangle( c
, tri
, co
);
1439 v3_sub( c
, co
, delta
);
1442 float d
= v3_length2( delta
);
1446 v3_sub( tri
[1], tri
[0], ab
);
1447 v3_sub( tri
[2], tri
[0], ac
);
1448 v3_cross( ac
, ab
, tn
);
1450 if( v3_dot( delta
, tn
) > 0.0f
)
1451 v3_muls( delta
, -1.0f
, delta
);
1453 vg_line_pt3( co
, 0.05f
, 0xff00ff00 );
1456 v3_muls( delta
, 1.0f
/d
, norm
);
1468 static void rb_solver_reset(void)
1470 rb_contact_count
= 0;
1473 static rb_ct
*rb_global_ct(void)
1475 return rb_contact_buffer
+ rb_contact_count
;
1479 static struct contact
*rb_start_contact(void)
1481 if( rb_contact_count
== vg_list_size(rb_contact_buffer
) )
1483 vg_error( "rigidbody: too many contacts generated (%u)\n",
1488 return &rb_contact_buffer
[ rb_contact_count
];
1491 static void rb_commit_contact( struct contact
*ct
, float p
)
1493 ct
->bias
= -0.2f
*k_rb_rate
*vg_minf(0.0f
,-p
+0.04f
);
1494 rb_tangent_basis( ct
->n
, ct
->t
[0], ct
->t
[1] );
1496 ct
->norm_impulse
= 0.0f
;
1497 ct
->tangent_impulse
[0] = 0.0f
;
1498 ct
->tangent_impulse
[1] = 0.0f
;
1500 rb_contact_count
++;
1503 static void rb_build_manifold_terrain_sphere( rigidbody
*rb
)
1507 int len
= bh_select( &world
.geo
.bhtris
, rb
->bbx_world
, geo
, 256 );
1509 for( int i
=0; i
<len
; i
++ )
1511 u32
*ptri
= &world
.geo
.indices
[ geo
[i
]*3 ];
1513 for( int j
=0; j
<3; j
++ )
1514 v3_copy( world
.geo
.verts
[ptri
[j
]].co
, tri
[j
] );
1516 vg_line(tri
[0],tri
[1],0xff00ff00 );
1517 vg_line(tri
[1],tri
[2],0xff00ff00 );
1518 vg_line(tri
[2],tri
[0],0xff00ff00 );
1523 for( int j
=0; j
<2; j
++ )
1525 if( sphere_vs_triangle( rb
->co
, rb
->inf
.sphere
.radius
, tri
,co
,norm
,&p
))
1527 struct contact
*ct
= rb_start_contact();
1533 v3_muladds( rb
->co
, norm
, p
, p1
);
1534 vg_line( rb
->co
, p1
, 0xffffffff );
1537 v3_copy( co
, ct
->co
);
1538 v3_copy( norm
, ct
->n
);
1539 rb_commit_contact( ct
, p
);
1546 static void rb_build_manifold_terrain( rigidbody
*rb
)
1550 float *p000
= pts
[0], *p001
= pts
[1], *p010
= pts
[2], *p011
= pts
[3],
1551 *p100
= pts
[4], *p101
= pts
[5], *p110
= pts
[6], *p111
= pts
[7];
1553 p000
[0]=box
[0][0];p000
[1]=box
[0][1];p000
[2]=box
[0][2];
1554 p001
[0]=box
[0][0];p001
[1]=box
[0][1];p001
[2]=box
[1][2];
1555 p010
[0]=box
[0][0];p010
[1]=box
[1][1];p010
[2]=box
[0][2];
1556 p011
[0]=box
[0][0];p011
[1]=box
[1][1];p011
[2]=box
[1][2];
1558 p100
[0]=box
[1][0];p100
[1]=box
[0][1];p100
[2]=box
[0][2];
1559 p101
[0]=box
[1][0];p101
[1]=box
[0][1];p101
[2]=box
[1][2];
1560 p110
[0]=box
[1][0];p110
[1]=box
[1][1];p110
[2]=box
[0][2];
1561 p111
[0]=box
[1][0];p111
[1]=box
[1][1];p111
[2]=box
[1][2];
1563 m4x3_mulv( rb
->to_world
, p000
, p000
);
1564 m4x3_mulv( rb
->to_world
, p001
, p001
);
1565 m4x3_mulv( rb
->to_world
, p010
, p010
);
1566 m4x3_mulv( rb
->to_world
, p011
, p011
);
1567 m4x3_mulv( rb
->to_world
, p100
, p100
);
1568 m4x3_mulv( rb
->to_world
, p101
, p101
);
1569 m4x3_mulv( rb
->to_world
, p110
, p110
);
1570 m4x3_mulv( rb
->to_world
, p111
, p111
);
1574 for( int i
=0; i
<8; i
++ )
1576 float *point
= pts
[i
];
1577 struct contact
*ct
= rb_start_contact();
1585 v3_copy( point
, surface
);
1589 hit
.dist
= INFINITY
;
1590 if( !ray_world( surface
, (v3f
){0.0f
,-1.0f
,0.0f
}, &hit
))
1593 v3_copy( hit
.pos
, surface
);
1595 float p
= vg_minf( surface
[1] - point
[1], 1.0f
);
1599 v3_copy( hit
.normal
, ct
->n
);
1600 v3_add( point
, surface
, ct
->co
);
1601 v3_muls( ct
->co
, 0.5f
, ct
->co
);
1603 rb_commit_contact( ct
, p
);
1613 * Initializing things like tangent vectors
1616 static void rb_presolve_contacts( rb_ct
*buffer
, int len
)
1618 for( int i
=0; i
<len
; i
++ )
1620 rb_ct
*ct
= &buffer
[i
];
1621 ct
->bias
= -0.2f
* k_rb_rate
* vg_minf(0.0f
,-ct
->p
+0.01f
);
1622 rb_tangent_basis( ct
->n
, ct
->t
[0], ct
->t
[1] );
1624 ct
->norm_impulse
= 0.0f
;
1625 ct
->tangent_impulse
[0] = 0.0f
;
1626 ct
->tangent_impulse
[1] = 0.0f
;
1628 v3f ra
, rb
, raCn
, rbCn
, raCt
, rbCt
;
1629 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
1630 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
1631 v3_cross( ra
, ct
->n
, raCn
);
1632 v3_cross( rb
, ct
->n
, rbCn
);
1634 /* orient inverse inertia tensors */
1636 m3x3_mulv( ct
->rba
->iIw
, raCn
, raCnI
);
1637 m3x3_mulv( ct
->rbb
->iIw
, rbCn
, rbCnI
);
1639 ct
->normal_mass
= ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
1640 ct
->normal_mass
+= v3_dot( raCn
, raCnI
);
1641 ct
->normal_mass
+= v3_dot( rbCn
, rbCnI
);
1642 ct
->normal_mass
= 1.0f
/ct
->normal_mass
;
1644 for( int j
=0; j
<2; j
++ )
1647 v3_cross( ct
->t
[j
], ra
, raCt
);
1648 v3_cross( ct
->t
[j
], rb
, rbCt
);
1649 m3x3_mulv( ct
->rba
->iIw
, raCt
, raCtI
);
1650 m3x3_mulv( ct
->rbb
->iIw
, rbCt
, rbCtI
);
1652 ct
->tangent_mass
[j
] = ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
1653 ct
->tangent_mass
[j
] += v3_dot( raCt
, raCtI
);
1654 ct
->tangent_mass
[j
] += v3_dot( rbCt
, rbCtI
);
1655 ct
->tangent_mass
[j
] = 1.0f
/ct
->tangent_mass
[j
];
1658 rb_debug_contact( ct
);
1663 * Creates relative contact velocity vector, and offsets between each body
1665 static void rb_rcv( rb_ct
*ct
, v3f rv
, v3f da
, v3f db
)
1667 rigidbody
*rba
= ct
->rba
,
1670 v3_sub( ct
->co
, rba
->co
, da
);
1671 v3_sub( ct
->co
, rbb
->co
, db
);
1674 v3_cross( rba
->w
, da
, rva
);
1675 v3_add( rba
->v
, rva
, rva
);
1676 v3_cross( rbb
->w
, db
, rvb
);
1677 v3_add( rbb
->v
, rvb
, rvb
);
1679 v3_sub( rva
, rvb
, rv
);
1683 * Apply regular and angular velocity impulses to objects involved in contact
1686 /* TODO REMOVEEE................... */
1687 static void rb_standard_impulse( rb_ct
*ct
, v3f da
, v3f db
, v3f impulse
)
1689 rigidbody
*rba
= ct
->rba
,
1692 v3_muladds( rba
->v
, impulse
, rba
->inv_mass
, rba
->v
);
1693 v3_muladds( rbb
->v
, impulse
, -rbb
->inv_mass
, rbb
->v
);
1695 /* Angular velocity */
1697 v3_cross( da
, impulse
, wa
);
1698 v3_negate( impulse
, invim
);
1699 v3_cross( db
, invim
, wb
);
1701 m3x3_mulv( ct
->rba
->iIw
, wa
, wa
);
1702 m3x3_mulv( ct
->rbb
->iIw
, wb
, wb
);
1703 v3_add( rba
->w
, wa
, rba
->w
);
1704 v3_add( rbb
->w
, wb
, rbb
->w
);
1707 /* ......... USE THIS */
1708 static void rb_linear_impulse( rigidbody
*rb
, v3f delta
, v3f impulse
)
1711 v3_muladds( rb
->v
, impulse
, rb
->inv_mass
, rb
->v
);
1713 /* Angular velocity */
1715 v3_cross( delta
, impulse
, wa
);
1717 m3x3_mulv( rb
->iIw
, wa
, wa
);
1718 v3_add( rb
->w
, wa
, rb
->w
);
1722 * One iteration to solve the contact constraint
1724 static void rb_solve_contacts( rb_ct
*buf
, int len
)
1726 float k_friction
= 0.2f
;
1728 for( int i
=0; i
<len
; i
++ )
1730 struct contact
*ct
= &buf
[i
];
1731 rigidbody
*rb
= ct
->rba
;
1734 rb_rcv( ct
, rv
, da
, db
);
1737 for( int j
=0; j
<2; j
++ )
1739 float f
= k_friction
* ct
->norm_impulse
,
1740 vt
= v3_dot( rv
, ct
->t
[j
] ),
1741 lambda
= ct
->tangent_mass
[j
] * -vt
;
1743 float temp
= ct
->tangent_impulse
[j
];
1744 ct
->tangent_impulse
[j
] = vg_clampf( temp
+ lambda
, -f
, f
);
1745 lambda
= ct
->tangent_impulse
[j
] - temp
;
1748 v3_muls( ct
->t
[j
], lambda
, impulse
);
1749 rb_standard_impulse( ct
, da
, db
, impulse
);
1753 rb_rcv( ct
, rv
, da
, db
);
1754 float vn
= v3_dot( rv
, ct
->n
),
1755 lambda
= ct
->normal_mass
* (-vn
+ ct
->bias
);
1757 float temp
= ct
->norm_impulse
;
1758 ct
->norm_impulse
= vg_maxf( temp
+ lambda
, 0.0f
);
1759 lambda
= ct
->norm_impulse
- temp
;
1762 v3_muls( ct
->n
, lambda
, impulse
);
1763 rb_standard_impulse( ct
, da
, db
, impulse
);
1768 * The following ventures into not really very sophisticated at all maths
1771 struct rb_angle_limit
1773 rigidbody
*rba
, *rbb
;
1775 float impulse
, bias
;
1778 static int rb_angle_limit_force( rigidbody
*rba
, v3f va
,
1779 rigidbody
*rbb
, v3f vb
,
1783 m3x3_mulv( rba
->to_world
, va
, wva
);
1784 m3x3_mulv( rbb
->to_world
, vb
, wvb
);
1786 float dt
= v3_dot(wva
,wvb
)*0.999f
,
1791 float correction
= max
-ang
;
1794 v3_cross( wva
, wvb
, axis
);
1797 q_axis_angle( rotation
, axis
, -correction
*0.25f
);
1798 q_mul( rotation
, rba
->q
, rba
->q
);
1800 q_axis_angle( rotation
, axis
, correction
*0.25f
);
1801 q_mul( rotation
, rbb
->q
, rbb
->q
);
1809 static void rb_constraint_angle_limit( struct rb_angle_limit
*limit
)
1814 static void rb_constraint_angle( rigidbody
*rba
, v3f va
,
1815 rigidbody
*rbb
, v3f vb
,
1816 float max
, float spring
)
1819 m3x3_mulv( rba
->to_world
, va
, wva
);
1820 m3x3_mulv( rbb
->to_world
, vb
, wvb
);
1822 float dt
= v3_dot(wva
,wvb
)*0.999f
,
1826 v3_cross( wva
, wvb
, axis
);
1827 v3_muladds( rba
->w
, axis
, ang
*spring
*0.5f
, rba
->w
);
1828 v3_muladds( rbb
->w
, axis
, -ang
*spring
*0.5f
, rbb
->w
);
1832 /* TODO: convert max into the dot product value so we dont have to always
1833 * evaluate acosf, only if its greater than the angle specified */
1837 float correction
= max
-ang
;
1840 q_axis_angle( rotation
, axis
, -correction
*0.125f
);
1841 q_mul( rotation
, rba
->q
, rba
->q
);
1843 q_axis_angle( rotation
, axis
, correction
*0.125f
);
1844 q_mul( rotation
, rbb
->q
, rbb
->q
);
1848 static void draw_angle_limit( v3f c
, v3f major
, v3f minor
,
1849 float amin
, float amax
, float measured
,
1854 v3_muls( major
, f
, ay
);
1855 v3_muls( minor
, f
, ax
);
1857 for( int x
=0; x
<16; x
++ )
1859 float t0
= (float)x
/ 16.0f
,
1860 t1
= (float)(x
+1) / 16.0f
,
1861 a0
= vg_lerpf( amin
, amax
, t0
),
1862 a1
= vg_lerpf( amin
, amax
, t1
);
1865 v3_muladds( c
, ay
, cosf(a0
), p0
);
1866 v3_muladds( p0
, ax
, sinf(a0
), p0
);
1867 v3_muladds( c
, ay
, cosf(a1
), p1
);
1868 v3_muladds( p1
, ax
, sinf(a1
), p1
);
1870 vg_line( p0
, p1
, colour
);
1873 vg_line( c
, p0
, colour
);
1875 vg_line( c
, p1
, colour
);
1879 v3_muladds( c
, ay
, cosf(measured
)*1.2f
, p2
);
1880 v3_muladds( p2
, ax
, sinf(measured
)*1.2f
, p2
);
1881 vg_line( c
, p2
, colour
);
1884 static void rb_debug_constraint_limits( rigidbody
*ra
, rigidbody
*rb
, v3f lca
,
1887 v3f ax
, ay
, az
, bx
, by
, bz
;
1888 m3x3_mulv( ra
->to_world
, (v3f
){1.0f
,0.0f
,0.0f
}, ax
);
1889 m3x3_mulv( ra
->to_world
, (v3f
){0.0f
,1.0f
,0.0f
}, ay
);
1890 m3x3_mulv( ra
->to_world
, (v3f
){0.0f
,0.0f
,1.0f
}, az
);
1891 m3x3_mulv( rb
->to_world
, (v3f
){1.0f
,0.0f
,0.0f
}, bx
);
1892 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,1.0f
,0.0f
}, by
);
1893 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,0.0f
,1.0f
}, bz
);
1896 px
[0] = v3_dot( ay
, by
);
1897 px
[1] = v3_dot( az
, by
);
1899 py
[0] = v3_dot( az
, bz
);
1900 py
[1] = v3_dot( ax
, bz
);
1902 pz
[0] = v3_dot( ax
, bx
);
1903 pz
[1] = v3_dot( ay
, bx
);
1905 float r0
= atan2f( px
[1], px
[0] ),
1906 r1
= atan2f( py
[1], py
[0] ),
1907 r2
= atan2f( pz
[1], pz
[0] );
1910 m4x3_mulv( ra
->to_world
, lca
, c
);
1911 draw_angle_limit( c
, ay
, az
, limits
[0][0], limits
[1][0], r0
, 0xff0000ff );
1912 draw_angle_limit( c
, az
, ax
, limits
[0][1], limits
[1][1], r1
, 0xff00ff00 );
1913 draw_angle_limit( c
, ax
, ay
, limits
[0][2], limits
[1][2], r2
, 0xffff0000 );
1916 static void rb_limit_cure( rigidbody
*ra
, rigidbody
*rb
, v3f axis
, float d
)
1920 float avx
= v3_dot( ra
->w
, axis
) - v3_dot( rb
->w
, axis
);
1921 float joint_mass
= rb
->inv_mass
+ ra
->inv_mass
;
1922 joint_mass
= 1.0f
/joint_mass
;
1924 float bias
= (0.04f
* k_rb_rate
) * d
,
1925 lambda
= -(avx
+ bias
) * joint_mass
;
1927 /* Angular velocity */
1929 v3_muls( axis
, lambda
* ra
->inv_mass
, wa
);
1930 v3_muls( axis
, -lambda
* rb
->inv_mass
, wb
);
1932 v3_add( ra
->w
, wa
, ra
->w
);
1933 v3_add( rb
->w
, wb
, rb
->w
);
1937 static void rb_constraint_limits( rigidbody
*ra
, v3f lca
,
1938 rigidbody
*rb
, v3f lcb
, v3f limits
[2] )
1940 /* TODO: Code dupe remover */
1941 v3f ax
, ay
, az
, bx
, by
, bz
;
1942 m3x3_mulv( ra
->to_world
, (v3f
){1.0f
,0.0f
,0.0f
}, ax
);
1943 m3x3_mulv( ra
->to_world
, (v3f
){0.0f
,1.0f
,0.0f
}, ay
);
1944 m3x3_mulv( ra
->to_world
, (v3f
){0.0f
,0.0f
,1.0f
}, az
);
1945 m3x3_mulv( rb
->to_world
, (v3f
){1.0f
,0.0f
,0.0f
}, bx
);
1946 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,1.0f
,0.0f
}, by
);
1947 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,0.0f
,1.0f
}, bz
);
1950 px
[0] = v3_dot( ay
, by
);
1951 px
[1] = v3_dot( az
, by
);
1953 py
[0] = v3_dot( az
, bz
);
1954 py
[1] = v3_dot( ax
, bz
);
1956 pz
[0] = v3_dot( ax
, bx
);
1957 pz
[1] = v3_dot( ay
, bx
);
1959 float r0
= atan2f( px
[1], px
[0] ),
1960 r1
= atan2f( py
[1], py
[0] ),
1961 r2
= atan2f( pz
[1], pz
[0] );
1963 /* calculate angle deltas */
1964 float dx
= 0.0f
, dy
= 0.0f
, dz
= 0.0f
;
1966 if( r0
< limits
[0][0] ) dx
= limits
[0][0] - r0
;
1967 if( r0
> limits
[1][0] ) dx
= limits
[1][0] - r0
;
1968 if( r1
< limits
[0][1] ) dy
= limits
[0][1] - r1
;
1969 if( r1
> limits
[1][1] ) dy
= limits
[1][1] - r1
;
1970 if( r2
< limits
[0][2] ) dz
= limits
[0][2] - r2
;
1971 if( r2
> limits
[1][2] ) dz
= limits
[1][2] - r2
;
1974 m3x3_mulv( ra
->to_world
, lca
, wca
);
1975 m3x3_mulv( rb
->to_world
, lcb
, wcb
);
1977 rb_limit_cure( ra
, rb
, ax
, dx
);
1978 rb_limit_cure( ra
, rb
, ay
, dy
);
1979 rb_limit_cure( ra
, rb
, az
, dz
);
1982 static void rb_debug_constraint_position( rigidbody
*ra
, v3f lca
,
1983 rigidbody
*rb
, v3f lcb
)
1986 m3x3_mulv( ra
->to_world
, lca
, wca
);
1987 m3x3_mulv( rb
->to_world
, lcb
, wcb
);
1990 v3_add( wca
, ra
->co
, p0
);
1991 v3_add( wcb
, rb
->co
, p1
);
1992 vg_line_pt3( p0
, 0.005f
, 0xffffff00 );
1993 vg_line_pt3( p1
, 0.005f
, 0xffffff00 );
1994 vg_line( p0
, p1
, 0xffffff00 );
1997 static void rb_constraint_position( rigidbody
*ra
, v3f lca
,
1998 rigidbody
*rb
, v3f lcb
)
2000 /* C = (COa + Ra*LCa) - (COb + Rb*LCb) = 0 */
2002 m3x3_mulv( ra
->to_world
, lca
, wca
);
2003 m3x3_mulv( rb
->to_world
, lcb
, wcb
);
2006 v3_sub( ra
->v
, rb
->v
, rcv
);
2009 v3_cross( ra
->w
, wca
, rcv_Ra
);
2010 v3_cross( rb
->w
, wcb
, rcv_Rb
);
2011 v3_add( rcv_Ra
, rcv
, rcv
);
2012 v3_sub( rcv
, rcv_Rb
, rcv
);
2016 v3_add( wca
, ra
->co
, p0
);
2017 v3_add( wcb
, rb
->co
, p1
);
2018 v3_sub( p1
, p0
, delta
);
2020 float dist2
= v3_length2( delta
);
2022 if( dist2
> 0.00001f
)
2024 float dist
= sqrtf(dist2
);
2025 v3_muls( delta
, 1.0f
/dist
, delta
);
2027 float joint_mass
= rb
->inv_mass
+ ra
->inv_mass
;
2029 v3f raCn
, rbCn
, raCt
, rbCt
;
2030 v3_cross( wca
, delta
, raCn
);
2031 v3_cross( wcb
, delta
, rbCn
);
2033 /* orient inverse inertia tensors */
2035 m3x3_mulv( ra
->iIw
, raCn
, raCnI
);
2036 m3x3_mulv( rb
->iIw
, rbCn
, rbCnI
);
2037 joint_mass
+= v3_dot( raCn
, raCnI
);
2038 joint_mass
+= v3_dot( rbCn
, rbCnI
);
2039 joint_mass
= 1.0f
/joint_mass
;
2041 float vd
= v3_dot( rcv
, delta
),
2042 bias
= -(0.08f
* k_rb_rate
) * dist
,
2043 lambda
= -(vd
+ bias
) * joint_mass
;
2046 v3_muls( delta
, lambda
, impulse
);
2047 rb_linear_impulse( ra
, wca
, impulse
);
2048 v3_muls( delta
, -lambda
, impulse
);
2049 rb_linear_impulse( rb
, wcb
, impulse
);
2052 v3_muladds( ra
->co
, delta
, dist
* 0.01f
, ra
->co
);
2053 v3_muladds( rb
->co
, delta
, -dist
* 0.01f
, rb
->co
);
2057 static void debug_sphere( m4x3f m
, float radius
, u32 colour
)
2059 v3f ly
= { 0.0f
, 0.0f
, radius
},
2060 lx
= { 0.0f
, radius
, 0.0f
},
2061 lz
= { 0.0f
, 0.0f
, radius
};
2063 for( int i
=0; i
<16; i
++ )
2065 float t
= ((float)(i
+1) * (1.0f
/16.0f
)) * VG_PIf
* 2.0f
,
2069 v3f py
= { s
*radius
, 0.0f
, c
*radius
},
2070 px
= { s
*radius
, c
*radius
, 0.0f
},
2071 pz
= { 0.0f
, s
*radius
, c
*radius
};
2073 v3f p0
, p1
, p2
, p3
, p4
, p5
;
2074 m4x3_mulv( m
, py
, p0
);
2075 m4x3_mulv( m
, ly
, p1
);
2076 m4x3_mulv( m
, px
, p2
);
2077 m4x3_mulv( m
, lx
, p3
);
2078 m4x3_mulv( m
, pz
, p4
);
2079 m4x3_mulv( m
, lz
, p5
);
2081 vg_line( p0
, p1
, colour
== 0x00? 0xff00ff00: colour
);
2082 vg_line( p2
, p3
, colour
== 0x00? 0xff0000ff: colour
);
2083 vg_line( p4
, p5
, colour
== 0x00? 0xffff0000: colour
);
2091 static void debug_capsule( m4x3f m
, float radius
, float h
, u32 colour
)
2093 v3f ly
= { 0.0f
, 0.0f
, radius
},
2094 lx
= { 0.0f
, radius
, 0.0f
},
2095 lz
= { 0.0f
, 0.0f
, radius
};
2097 float s0
= sinf(0.0f
)*radius
,
2098 c0
= cosf(0.0f
)*radius
;
2100 v3f p0
, p1
, up
, right
, forward
;
2101 m3x3_mulv( m
, (v3f
){0.0f
,1.0f
,0.0f
}, up
);
2102 m3x3_mulv( m
, (v3f
){1.0f
,0.0f
,0.0f
}, right
);
2103 m3x3_mulv( m
, (v3f
){0.0f
,0.0f
,-1.0f
}, forward
);
2104 v3_muladds( m
[3], up
, -h
*0.5f
+radius
, p0
);
2105 v3_muladds( m
[3], up
, h
*0.5f
-radius
, p1
);
2108 v3_muladds( p0
, right
, radius
, a0
);
2109 v3_muladds( p1
, right
, radius
, a1
);
2110 v3_muladds( p0
, forward
, radius
, b0
);
2111 v3_muladds( p1
, forward
, radius
, b1
);
2112 vg_line( a0
, a1
, colour
);
2113 vg_line( b0
, b1
, colour
);
2115 v3_muladds( p0
, right
, -radius
, a0
);
2116 v3_muladds( p1
, right
, -radius
, a1
);
2117 v3_muladds( p0
, forward
, -radius
, b0
);
2118 v3_muladds( p1
, forward
, -radius
, b1
);
2119 vg_line( a0
, a1
, colour
);
2120 vg_line( b0
, b1
, colour
);
2122 for( int i
=0; i
<16; i
++ )
2124 float t
= ((float)(i
+1) * (1.0f
/16.0f
)) * VG_PIf
* 2.0f
,
2125 s1
= sinf(t
)*radius
,
2126 c1
= cosf(t
)*radius
;
2128 v3f e0
= { s0
, 0.0f
, c0
},
2129 e1
= { s1
, 0.0f
, c1
},
2130 e2
= { s0
, c0
, 0.0f
},
2131 e3
= { s1
, c1
, 0.0f
},
2132 e4
= { 0.0f
, c0
, s0
},
2133 e5
= { 0.0f
, c1
, s1
};
2135 m3x3_mulv( m
, e0
, e0
);
2136 m3x3_mulv( m
, e1
, e1
);
2137 m3x3_mulv( m
, e2
, e2
);
2138 m3x3_mulv( m
, e3
, e3
);
2139 m3x3_mulv( m
, e4
, e4
);
2140 m3x3_mulv( m
, e5
, e5
);
2142 v3_add( p0
, e0
, a0
);
2143 v3_add( p0
, e1
, a1
);
2144 v3_add( p1
, e0
, b0
);
2145 v3_add( p1
, e1
, b1
);
2147 vg_line( a0
, a1
, colour
);
2148 vg_line( b0
, b1
, colour
);
2152 v3_add( p0
, e2
, a0
);
2153 v3_add( p0
, e3
, a1
);
2154 v3_add( p0
, e4
, b0
);
2155 v3_add( p0
, e5
, b1
);
2159 v3_add( p1
, e2
, a0
);
2160 v3_add( p1
, e3
, a1
);
2161 v3_add( p1
, e4
, b0
);
2162 v3_add( p1
, e5
, b1
);
2165 vg_line( a0
, a1
, colour
);
2166 vg_line( b0
, b1
, colour
);
2173 static void rb_debug( rigidbody
*rb
, u32 colour
)
2175 if( rb
->type
== k_rb_shape_box
)
2178 vg_line_boxf_transformed( rb
->to_world
, rb
->bbx
, colour
);
2180 else if( rb
->type
== k_rb_shape_sphere
)
2182 debug_sphere( rb
->to_world
, rb
->inf
.sphere
.radius
, colour
);
2184 else if( rb
->type
== k_rb_shape_capsule
)
2187 float h
= rb
->inf
.capsule
.height
,
2188 r
= rb
->inf
.capsule
.radius
;
2190 debug_capsule( rb
->to_world
, r
, h
, colour
);
2192 else if( rb
->type
== k_rb_shape_scene
)
2194 vg_line_boxf( rb
->bbx
, colour
);
2200 * out penetration distance, normal
2202 static int rb_point_in_body( rigidbody
*rb
, v3f pos
, float *pen
, v3f normal
)
2205 m4x3_mulv( rb
->to_local
, pos
, local
);
2207 if( local
[0] > rb
->bbx
[0][0] && local
[0] < rb
->bbx
[1][0] &&
2208 local
[1] > rb
->bbx
[0][1] && local
[1] < rb
->bbx
[1][1] &&
2209 local
[2] > rb
->bbx
[0][2] && local
[2] < rb
->bbx
[1][2] )
2211 v3f area
, com
, comrel
;
2212 v3_add( rb
->bbx
[0], rb
->bbx
[1], com
);
2213 v3_muls( com
, 0.5f
, com
);
2215 v3_sub( rb
->bbx
[1], rb
->bbx
[0], area
);
2216 v3_sub( local
, com
, comrel
);
2217 v3_div( comrel
, area
, comrel
);
2220 float max_mag
= fabsf(comrel
[0]);
2222 if( fabsf(comrel
[1]) > max_mag
)
2225 max_mag
= fabsf(comrel
[1]);
2227 if( fabsf(comrel
[2]) > max_mag
)
2230 max_mag
= fabsf(comrel
[2]);
2234 normal
[axis
] = vg_signf(comrel
[axis
]);
2236 if( normal
[axis
] < 0.0f
)
2237 *pen
= local
[axis
] - rb
->bbx
[0][axis
];
2239 *pen
= rb
->bbx
[1][axis
] - local
[axis
];
2241 m3x3_mulv( rb
->to_world
, normal
, normal
);
2249 * BVH implementation, this is ONLY for static rigidbodies, its to slow for
2253 static void rb_bh_expand_bound( void *user
, boxf bound
, u32 item_index
)
2255 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
2256 box_concat( bound
, rb
->bbx_world
);
2259 static float rb_bh_centroid( void *user
, u32 item_index
, int axis
)
2261 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
2262 return (rb
->bbx_world
[axis
][0] + rb
->bbx_world
[1][axis
]) * 0.5f
;
2265 static void rb_bh_swap( void *user
, u32 ia
, u32 ib
)
2267 rigidbody temp
, *rba
, *rbb
;
2268 rba
= &((rigidbody
*)user
)[ ia
];
2269 rbb
= &((rigidbody
*)user
)[ ib
];
2276 static void rb_bh_debug( void *user
, u32 item_index
)
2278 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
2279 rb_debug( rb
, 0xff00ffff );
2282 static bh_system bh_system_rigidbodies
=
2284 .expand_bound
= rb_bh_expand_bound
,
2285 .item_centroid
= rb_bh_centroid
,
2286 .item_swap
= rb_bh_swap
,
2287 .item_debug
= rb_bh_debug
,
2293 #endif /* RIGIDBODY_H */