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 * -----------------------------------------------------------------------------
19 * -----------------------------------------------------------------------------
24 k_rb_delta
= (1.0f
/k_rb_rate
),
26 k_damp_linear
= 0.05f
, /* scale velocity 1/(1+x) */
27 k_damp_angular
= 0.1f
, /* scale angular 1/(1+x) */
29 k_joint_bias
= 0.08f
, /* positional joints */
30 k_joint_correction
= 0.01f
,
31 k_penetration_slop
= 0.01f
,
32 k_inertia_scale
= 4.0f
;
35 * -----------------------------------------------------------------------------
36 * structure definitions
37 * -----------------------------------------------------------------------------
40 typedef struct rigidbody rigidbody
;
41 typedef struct contact rb_ct
;
51 k_rb_shape_sphere
= 1,
52 k_rb_shape_capsule
= 2,
79 v3f right
, up
, forward
;
86 /* inertia model and inverse world tensor */
90 m4x3f to_world
, to_local
;
98 float p
, bias
, norm_impulse
, tangent_impulse
[2],
99 normal_mass
, tangent_mass
[2];
103 rb_contact_buffer
[256];
104 static int rb_contact_count
= 0;
107 * -----------------------------------------------------------------------------
109 * -----------------------------------------------------------------------------
112 static float sphere_volume( float radius
)
114 float r3
= radius
*radius
*radius
;
115 return (4.0f
/3.0f
) * VG_PIf
* r3
;
118 static void rb_tangent_basis( v3f n
, v3f tx
, v3f ty
)
120 /* Compute tangent basis (box2d) */
121 if( fabsf( n
[0] ) >= 0.57735027f
)
135 v3_cross( n
, tx
, ty
);
139 * -----------------------------------------------------------------------------
141 * -----------------------------------------------------------------------------
144 static void rb_debug_contact( rb_ct
*ct
)
147 v3_muladds( ct
->co
, ct
->n
, 0.1f
, p1
);
148 vg_line_pt3( ct
->co
, 0.025f
, 0xff0000ff );
149 vg_line( ct
->co
, p1
, 0xffffffff );
152 static void debug_sphere( m4x3f m
, float radius
, u32 colour
)
154 v3f ly
= { 0.0f
, 0.0f
, radius
},
155 lx
= { 0.0f
, radius
, 0.0f
},
156 lz
= { 0.0f
, 0.0f
, radius
};
158 for( int i
=0; i
<16; i
++ )
160 float t
= ((float)(i
+1) * (1.0f
/16.0f
)) * VG_PIf
* 2.0f
,
164 v3f py
= { s
*radius
, 0.0f
, c
*radius
},
165 px
= { s
*radius
, c
*radius
, 0.0f
},
166 pz
= { 0.0f
, s
*radius
, c
*radius
};
168 v3f p0
, p1
, p2
, p3
, p4
, p5
;
169 m4x3_mulv( m
, py
, p0
);
170 m4x3_mulv( m
, ly
, p1
);
171 m4x3_mulv( m
, px
, p2
);
172 m4x3_mulv( m
, lx
, p3
);
173 m4x3_mulv( m
, pz
, p4
);
174 m4x3_mulv( m
, lz
, p5
);
176 vg_line( p0
, p1
, colour
== 0x00? 0xff00ff00: colour
);
177 vg_line( p2
, p3
, colour
== 0x00? 0xff0000ff: colour
);
178 vg_line( p4
, p5
, colour
== 0x00? 0xffff0000: colour
);
186 static void debug_capsule( m4x3f m
, float radius
, float h
, u32 colour
)
188 v3f ly
= { 0.0f
, 0.0f
, radius
},
189 lx
= { 0.0f
, radius
, 0.0f
},
190 lz
= { 0.0f
, 0.0f
, radius
};
192 float s0
= sinf(0.0f
)*radius
,
193 c0
= cosf(0.0f
)*radius
;
195 v3f p0
, p1
, up
, right
, forward
;
196 m3x3_mulv( m
, (v3f
){0.0f
,1.0f
,0.0f
}, up
);
197 m3x3_mulv( m
, (v3f
){1.0f
,0.0f
,0.0f
}, right
);
198 m3x3_mulv( m
, (v3f
){0.0f
,0.0f
,-1.0f
}, forward
);
199 v3_muladds( m
[3], up
, -h
*0.5f
+radius
, p0
);
200 v3_muladds( m
[3], up
, h
*0.5f
-radius
, p1
);
203 v3_muladds( p0
, right
, radius
, a0
);
204 v3_muladds( p1
, right
, radius
, a1
);
205 v3_muladds( p0
, forward
, radius
, b0
);
206 v3_muladds( p1
, forward
, radius
, b1
);
207 vg_line( a0
, a1
, colour
);
208 vg_line( b0
, b1
, colour
);
210 v3_muladds( p0
, right
, -radius
, a0
);
211 v3_muladds( p1
, right
, -radius
, a1
);
212 v3_muladds( p0
, forward
, -radius
, b0
);
213 v3_muladds( p1
, forward
, -radius
, b1
);
214 vg_line( a0
, a1
, colour
);
215 vg_line( b0
, b1
, colour
);
217 for( int i
=0; i
<16; i
++ )
219 float t
= ((float)(i
+1) * (1.0f
/16.0f
)) * VG_PIf
* 2.0f
,
223 v3f e0
= { s0
, 0.0f
, c0
},
224 e1
= { s1
, 0.0f
, c1
},
225 e2
= { s0
, c0
, 0.0f
},
226 e3
= { s1
, c1
, 0.0f
},
227 e4
= { 0.0f
, c0
, s0
},
228 e5
= { 0.0f
, c1
, s1
};
230 m3x3_mulv( m
, e0
, e0
);
231 m3x3_mulv( m
, e1
, e1
);
232 m3x3_mulv( m
, e2
, e2
);
233 m3x3_mulv( m
, e3
, e3
);
234 m3x3_mulv( m
, e4
, e4
);
235 m3x3_mulv( m
, e5
, e5
);
237 v3_add( p0
, e0
, a0
);
238 v3_add( p0
, e1
, a1
);
239 v3_add( p1
, e0
, b0
);
240 v3_add( p1
, e1
, b1
);
242 vg_line( a0
, a1
, colour
);
243 vg_line( b0
, b1
, colour
);
247 v3_add( p0
, e2
, a0
);
248 v3_add( p0
, e3
, a1
);
249 v3_add( p0
, e4
, b0
);
250 v3_add( p0
, e5
, b1
);
254 v3_add( p1
, e2
, a0
);
255 v3_add( p1
, e3
, a1
);
256 v3_add( p1
, e4
, b0
);
257 v3_add( p1
, e5
, b1
);
260 vg_line( a0
, a1
, colour
);
261 vg_line( b0
, b1
, colour
);
268 static void rb_debug( rigidbody
*rb
, u32 colour
)
270 if( rb
->type
== k_rb_shape_box
)
273 vg_line_boxf_transformed( rb
->to_world
, rb
->bbx
, colour
);
275 else if( rb
->type
== k_rb_shape_sphere
)
277 debug_sphere( rb
->to_world
, rb
->inf
.sphere
.radius
, colour
);
279 else if( rb
->type
== k_rb_shape_capsule
)
282 float h
= rb
->inf
.capsule
.height
,
283 r
= rb
->inf
.capsule
.radius
;
285 debug_capsule( rb
->to_world
, r
, h
, colour
);
287 else if( rb
->type
== k_rb_shape_scene
)
289 vg_line_boxf( rb
->bbx
, colour
);
294 * -----------------------------------------------------------------------------
296 * -----------------------------------------------------------------------------
300 * Update world space bounding box based on local one
302 static void rb_update_bounds( rigidbody
*rb
)
304 box_copy( rb
->bbx
, rb
->bbx_world
);
305 m4x3_transform_aabb( rb
->to_world
, rb
->bbx_world
);
309 * Commit transform to rigidbody. Updates matrices
311 static void rb_update_transform( rigidbody
*rb
)
313 q_normalize( rb
->q
);
314 q_m3x3( rb
->q
, rb
->to_world
);
315 v3_copy( rb
->co
, rb
->to_world
[3] );
317 m4x3_invert_affine( rb
->to_world
, rb
->to_local
);
319 m3x3_mulv( rb
->to_world
, (v3f
){1.0f
,0.0f
, 0.0f
}, rb
->right
);
320 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,1.0f
, 0.0f
}, rb
->up
);
321 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,0.0f
,-1.0f
}, rb
->forward
);
323 m3x3_mul( rb
->iI
, rb
->to_local
, rb
->iIw
);
324 m3x3_mul( rb
->to_world
, rb
->iIw
, rb
->iIw
);
326 rb_update_bounds( rb
);
330 * Initialize rigidbody and calculate masses, inertia
332 static void rb_init( rigidbody
*rb
)
336 if( rb
->type
== k_rb_shape_box
)
339 v3_sub( rb
->bbx
[1], rb
->bbx
[0], dims
);
340 volume
= dims
[0]*dims
[1]*dims
[2];
343 vg_info( "Box volume: %f\n", volume
);
345 else if( rb
->type
== k_rb_shape_sphere
)
347 volume
= sphere_volume( rb
->inf
.sphere
.radius
);
348 v3_fill( rb
->bbx
[0], -rb
->inf
.sphere
.radius
);
349 v3_fill( rb
->bbx
[1], rb
->inf
.sphere
.radius
);
351 vg_info( "Sphere volume: %f\n", volume
);
353 else if( rb
->type
== k_rb_shape_capsule
)
355 float r
= rb
->inf
.capsule
.radius
,
356 h
= rb
->inf
.capsule
.height
;
357 volume
= sphere_volume( r
) + VG_PIf
* r
*r
* (h
- r
*2.0f
);
359 v3_fill( rb
->bbx
[0], -rb
->inf
.sphere
.radius
);
360 v3_fill( rb
->bbx
[1], rb
->inf
.sphere
.radius
);
364 else if( rb
->type
== k_rb_shape_scene
)
367 box_copy( rb
->inf
.scene
.pscene
->bbx
, rb
->bbx
);
378 float mass
= 2.0f
*volume
;
379 rb
->inv_mass
= 1.0f
/mass
;
382 v3_sub( rb
->bbx
[1], rb
->bbx
[0], extent
);
383 v3_muls( extent
, 0.5f
, extent
);
385 /* local intertia tensor */
386 float scale
= k_inertia_scale
;
387 float ex2
= scale
*extent
[0]*extent
[0],
388 ey2
= scale
*extent
[1]*extent
[1],
389 ez2
= scale
*extent
[2]*extent
[2];
391 rb
->I
[0] = ((1.0f
/12.0f
) * mass
* (ey2
+ez2
));
392 rb
->I
[1] = ((1.0f
/12.0f
) * mass
* (ex2
+ez2
));
393 rb
->I
[2] = ((1.0f
/12.0f
) * mass
* (ex2
+ey2
));
395 m3x3_identity( rb
->iI
);
396 rb
->iI
[0][0] = rb
->I
[0];
397 rb
->iI
[1][1] = rb
->I
[1];
398 rb
->iI
[2][2] = rb
->I
[2];
399 m3x3_inv( rb
->iI
, rb
->iI
);
405 rb_update_transform( rb
);
408 static void rb_iter( rigidbody
*rb
)
410 v3f gravity
= { 0.0f
, -9.8f
, 0.0f
};
411 v3_muladds( rb
->v
, gravity
, k_rb_delta
, rb
->v
);
413 /* intergrate velocity */
414 v3_muladds( rb
->co
, rb
->v
, k_rb_delta
, rb
->co
);
415 v3_lerp( rb
->w
, (v3f
){0.0f
,0.0f
,0.0f
}, 0.0025f
, rb
->w
);
417 /* inegrate inertia */
418 if( v3_length2( rb
->w
) > 0.0f
)
422 v3_copy( rb
->w
, axis
);
424 float mag
= v3_length( axis
);
425 v3_divs( axis
, mag
, axis
);
426 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
);
427 q_mul( rotation
, rb
->q
, rb
->q
);
431 v3_muls( rb
->v
, 1.0f
/(1.0f
+k_rb_delta
*k_damp_linear
), rb
->v
);
432 v3_muls( rb
->w
, 1.0f
/(1.0f
+k_rb_delta
*k_damp_angular
), rb
->w
);
436 * -----------------------------------------------------------------------------
437 * Closest point functions
438 * -----------------------------------------------------------------------------
442 * These closest point tests were learned from Real-Time Collision Detection by
445 static float closest_segment_segment( v3f p1
, v3f q1
, v3f p2
, v3f q2
,
446 float *s
, float *t
, v3f c1
, v3f c2
)
449 v3_sub( q1
, p1
, d1
);
450 v3_sub( q2
, p2
, d2
);
453 float a
= v3_length2( d1
),
454 e
= v3_length2( d2
),
457 const float kEpsilon
= 0.0001f
;
459 if( a
<= kEpsilon
&& e
<= kEpsilon
)
467 v3_sub( c1
, c2
, v0
);
469 return v3_length2( v0
);
475 *t
= vg_clampf( f
/ e
, 0.0f
, 1.0f
);
479 float c
= v3_dot( d1
, r
);
483 *s
= vg_clampf( -c
/ a
, 0.0f
, 1.0f
);
487 float b
= v3_dot(d1
,d2
),
492 *s
= vg_clampf((b
*f
- c
*e
)/d
, 0.0f
, 1.0f
);
504 *s
= vg_clampf( -c
/ a
, 0.0f
, 1.0f
);
509 *s
= vg_clampf((b
-c
)/a
,0.0f
,1.0f
);
514 v3_muladds( p1
, d1
, *s
, c1
);
515 v3_muladds( p2
, d2
, *t
, c2
);
518 v3_sub( c1
, c2
, v0
);
519 return v3_length2( v0
);
522 static void closest_point_aabb( v3f p
, boxf box
, v3f dest
)
524 v3_maxv( p
, box
[0], dest
);
525 v3_minv( dest
, box
[1], dest
);
528 static void closest_point_obb( v3f p
, rigidbody
*rb
, v3f dest
)
531 m4x3_mulv( rb
->to_local
, p
, local
);
532 closest_point_aabb( local
, rb
->bbx
, local
);
533 m4x3_mulv( rb
->to_world
, local
, dest
);
536 static float closest_point_segment( v3f a
, v3f b
, v3f point
, v3f dest
)
540 v3_sub( point
, a
, v1
);
542 float t
= v3_dot( v1
, v0
) / v3_length2(v0
);
543 t
= vg_clampf(t
,0.0f
,1.0f
);
544 v3_muladds( a
, v0
, t
, dest
);
548 static void closest_on_triangle( v3f p
, v3f tri
[3], v3f dest
)
553 /* Region outside A */
554 v3_sub( tri
[1], tri
[0], ab
);
555 v3_sub( tri
[2], tri
[0], ac
);
556 v3_sub( p
, tri
[0], ap
);
560 if( d1
<= 0.0f
&& d2
<= 0.0f
)
562 v3_copy( tri
[0], dest
);
563 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
567 /* Region outside B */
571 v3_sub( p
, tri
[1], bp
);
572 d3
= v3_dot( ab
, bp
);
573 d4
= v3_dot( ac
, bp
);
575 if( d3
>= 0.0f
&& d4
<= d3
)
577 v3_copy( tri
[1], dest
);
578 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
582 /* Edge region of AB */
583 float vc
= d1
*d4
- d3
*d2
;
584 if( vc
<= 0.0f
&& d1
>= 0.0f
&& d3
<= 0.0f
)
586 float v
= d1
/ (d1
-d3
);
587 v3_muladds( tri
[0], ab
, v
, dest
);
588 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
592 /* Region outside C */
595 v3_sub( p
, tri
[2], cp
);
599 if( d6
>= 0.0f
&& d5
<= d6
)
601 v3_copy( tri
[2], dest
);
602 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
607 float vb
= d5
*d2
- d1
*d6
;
608 if( vb
<= 0.0f
&& d2
>= 0.0f
&& d6
<= 0.0f
)
610 float w
= d2
/ (d2
-d6
);
611 v3_muladds( tri
[0], ac
, w
, dest
);
612 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
617 float va
= d3
*d6
- d5
*d4
;
618 if( va
<= 0.0f
&& (d4
-d3
) >= 0.0f
&& (d5
-d6
) >= 0.0f
)
620 float w
= (d4
-d3
) / ((d4
-d3
) + (d5
-d6
));
622 v3_sub( tri
[2], tri
[1], bc
);
623 v3_muladds( tri
[1], bc
, w
, dest
);
624 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
628 /* P inside region, Q via barycentric coordinates uvw */
629 float d
= 1.0f
/(va
+vb
+vc
),
633 v3_muladds( tri
[0], ab
, v
, dest
);
634 v3_muladds( dest
, ac
, w
, dest
);
638 static void closest_on_triangle_1( v3f p
, v3f tri
[3], v3f dest
)
643 /* Region outside A */
644 v3_sub( tri
[1], tri
[0], ab
);
645 v3_sub( tri
[2], tri
[0], ac
);
646 v3_sub( p
, tri
[0], ap
);
650 if( d1
<= 0.0f
&& d2
<= 0.0f
)
652 v3_copy( tri
[0], dest
);
656 /* Region outside B */
660 v3_sub( p
, tri
[1], bp
);
661 d3
= v3_dot( ab
, bp
);
662 d4
= v3_dot( ac
, bp
);
664 if( d3
>= 0.0f
&& d4
<= d3
)
666 v3_copy( tri
[1], dest
);
670 /* Edge region of AB */
671 float vc
= d1
*d4
- d3
*d2
;
672 if( vc
<= 0.0f
&& d1
>= 0.0f
&& d3
<= 0.0f
)
674 float v
= d1
/ (d1
-d3
);
675 v3_muladds( tri
[0], ab
, v
, dest
);
679 /* Region outside C */
682 v3_sub( p
, tri
[2], cp
);
686 if( d6
>= 0.0f
&& d5
<= d6
)
688 v3_copy( tri
[2], dest
);
693 float vb
= d5
*d2
- d1
*d6
;
694 if( vb
<= 0.0f
&& d2
>= 0.0f
&& d6
<= 0.0f
)
696 float w
= d2
/ (d2
-d6
);
697 v3_muladds( tri
[0], ac
, w
, dest
);
702 float va
= d3
*d6
- d5
*d4
;
703 if( va
<= 0.0f
&& (d4
-d3
) >= 0.0f
&& (d5
-d6
) >= 0.0f
)
705 float w
= (d4
-d3
) / ((d4
-d3
) + (d5
-d6
));
707 v3_sub( tri
[2], tri
[1], bc
);
708 v3_muladds( tri
[1], bc
, w
, dest
);
712 /* P inside region, Q via barycentric coordinates uvw */
713 float d
= 1.0f
/(va
+vb
+vc
),
717 v3_muladds( tri
[0], ab
, v
, dest
);
718 v3_muladds( dest
, ac
, w
, dest
);
722 * -----------------------------------------------------------------------------
723 * Boolean shape overlap functions
724 * -----------------------------------------------------------------------------
728 * Project AABB, and triangle interval onto axis to check if they overlap
730 static int rb_box_triangle_interval( v3f extent
, v3f axis
, v3f tri
[3] )
734 r
= extent
[0] * fabsf(axis
[0]) +
735 extent
[1] * fabsf(axis
[1]) +
736 extent
[2] * fabsf(axis
[2]),
738 p0
= v3_dot( axis
, tri
[0] ),
739 p1
= v3_dot( axis
, tri
[1] ),
740 p2
= v3_dot( axis
, tri
[2] ),
742 e
= vg_maxf(-vg_maxf(p0
,vg_maxf(p1
,p2
)), vg_minf(p0
,vg_minf(p1
,p2
)));
744 if( e
> r
) return 0;
749 * Seperating axis test box vs triangle
751 static int rb_box_triangle_sat( rigidbody
*rba
, v3f tri_src
[3] )
756 v3_sub( rba
->bbx
[1], rba
->bbx
[0], extent
);
757 v3_muls( extent
, 0.5f
, extent
);
758 v3_add( rba
->bbx
[0], extent
, c
);
760 for( int i
=0; i
<3; i
++ )
762 m4x3_mulv( rba
->to_local
, tri_src
[i
], tri
[i
] );
763 v3_sub( tri
[i
], c
, tri
[i
] );
767 if(!rb_box_triangle_interval( extent
, (v3f
){1.0f
,0.0f
,0.0f
}, tri
)) return 0;
768 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,1.0f
,0.0f
}, tri
)) return 0;
769 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,0.0f
,1.0f
}, tri
)) return 0;
771 v3f v0
,v1
,v2
,n
, e0
,e1
,e2
;
772 v3_sub( tri
[1], tri
[0], v0
);
773 v3_sub( tri
[2], tri
[0], v1
);
774 v3_sub( tri
[2], tri
[1], v2
);
778 v3_cross( v0
, v1
, n
);
779 v3_cross( v0
, n
, e0
);
780 v3_cross( n
, v1
, e1
);
781 v3_cross( v2
, n
, e2
);
784 if(!rb_box_triangle_interval( extent
, n
, tri
)) return 0;
787 v3_cross( e0
, (v3f
){1.0f
,0.0f
,0.0f
}, axis
[0] );
788 v3_cross( e0
, (v3f
){0.0f
,1.0f
,0.0f
}, axis
[1] );
789 v3_cross( e0
, (v3f
){0.0f
,0.0f
,1.0f
}, axis
[2] );
790 v3_cross( e1
, (v3f
){1.0f
,0.0f
,0.0f
}, axis
[3] );
791 v3_cross( e1
, (v3f
){0.0f
,1.0f
,0.0f
}, axis
[4] );
792 v3_cross( e1
, (v3f
){0.0f
,0.0f
,1.0f
}, axis
[5] );
793 v3_cross( e2
, (v3f
){1.0f
,0.0f
,0.0f
}, axis
[6] );
794 v3_cross( e2
, (v3f
){0.0f
,1.0f
,0.0f
}, axis
[7] );
795 v3_cross( e2
, (v3f
){0.0f
,0.0f
,1.0f
}, axis
[8] );
797 for( int i
=0; i
<9; i
++ )
798 if(!rb_box_triangle_interval( extent
, axis
[i
], tri
)) return 0;
804 * -----------------------------------------------------------------------------
806 * -----------------------------------------------------------------------------
812 * These do not automatically allocate contacts, an appropriately sized
813 * buffer must be supplied. The function returns the size of the manifold
814 * which was generated.
816 * The values set on the contacts are: n, co, p, rba, rbb
820 * By collecting the minimum(time) and maximum(time) pairs of points, we
821 * build a reduced and stable exact manifold.
824 * rx: minimum distance of these points
825 * dx: the delta between the two points
827 * pairs will only ammend these if they are creating a collision
829 typedef struct capsule_manifold capsule_manifold
;
830 struct capsule_manifold
838 * Expand a line manifold with a new pair. t value is the time along segment
839 * on the oriented object which created this pair.
841 static void rb_capsule_manifold( v3f pa
, v3f pb
, float t
, float r
,
842 capsule_manifold
*manifold
)
845 v3_sub( pa
, pb
, delta
);
847 if( v3_length2(delta
) < r
*r
)
849 if( t
< manifold
->t0
)
851 v3_copy( delta
, manifold
->d0
);
856 if( t
> manifold
->t1
)
858 v3_copy( delta
, manifold
->d1
);
865 static void rb_capsule_manifold_init( capsule_manifold
*manifold
)
867 manifold
->t0
= INFINITY
;
868 manifold
->t1
= -INFINITY
;
871 static int rb_capsule_manifold_done( rigidbody
*rba
, rigidbody
*rbb
,
872 capsule_manifold
*manifold
, rb_ct
*buf
)
874 float h
= rba
->inf
.capsule
.height
,
875 ra
= rba
->inf
.capsule
.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 if( manifold
->t0
<= 1.0f
)
887 v3_muls( p0
, 1.0f
-manifold
->t0
, pa
);
888 v3_muladds( pa
, p1
, manifold
->t0
, pa
);
890 float d
= v3_length( manifold
->d0
);
891 v3_muls( manifold
->d0
, 1.0f
/d
, ct
->n
);
892 v3_muladds( pa
, ct
->n
, -ra
, ct
->co
);
894 ct
->p
= manifold
->r0
- d
;
901 if( (manifold
->t1
>= 0.0f
) && (manifold
->t0
!= manifold
->t1
) )
903 rb_ct
*ct
= buf
+count
;
906 v3_muls( p0
, 1.0f
-manifold
->t1
, pa
);
907 v3_muladds( pa
, p1
, manifold
->t1
, pa
);
909 float d
= v3_length( manifold
->d1
);
910 v3_muls( manifold
->d1
, 1.0f
/d
, ct
->n
);
911 v3_muladds( pa
, ct
->n
, -ra
, ct
->co
);
913 ct
->p
= manifold
->r1
- d
;
925 vg_line( buf
[0].co
, buf
[1].co
, 0xff0000ff );
930 static int rb_capsule_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
932 float h
= rba
->inf
.capsule
.height
,
933 ra
= rba
->inf
.capsule
.radius
,
934 rb
= rbb
->inf
.sphere
.radius
;
937 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+ra
, p0
);
938 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-ra
, p1
);
941 closest_point_segment( p0
, p1
, rbb
->co
, c
);
942 v3_sub( c
, rbb
->co
, delta
);
944 float d2
= v3_length2(delta
),
952 v3_muls( delta
, 1.0f
/d
, ct
->n
);
956 v3_muladds( c
, ct
->n
, -ra
, p0
);
957 v3_muladds( rbb
->co
, ct
->n
, rb
, p1
);
958 v3_add( p0
, p1
, ct
->co
);
959 v3_muls( ct
->co
, 0.5f
, ct
->co
);
970 static int rb_capsule_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
972 float ha
= rba
->inf
.capsule
.height
,
973 hb
= rbb
->inf
.capsule
.height
,
974 ra
= rba
->inf
.capsule
.radius
,
975 rb
= rbb
->inf
.capsule
.radius
,
979 v3_muladds( rba
->co
, rba
->up
, -ha
*0.5f
+ra
, p0
);
980 v3_muladds( rba
->co
, rba
->up
, ha
*0.5f
-ra
, p1
);
981 v3_muladds( rbb
->co
, rbb
->up
, -hb
*0.5f
+rb
, p2
);
982 v3_muladds( rbb
->co
, rbb
->up
, hb
*0.5f
-rb
, p3
);
984 capsule_manifold manifold
;
985 rb_capsule_manifold_init( &manifold
);
989 closest_segment_segment( p0
, p1
, p2
, p3
, &ta
, &tb
, pa
, pb
);
990 rb_capsule_manifold( pa
, pb
, ta
, r
, &manifold
);
992 ta
= closest_point_segment( p0
, p1
, p2
, pa
);
993 tb
= closest_point_segment( p0
, p1
, p3
, pb
);
994 rb_capsule_manifold( pa
, p2
, ta
, r
, &manifold
);
995 rb_capsule_manifold( pb
, p3
, tb
, r
, &manifold
);
997 closest_point_segment( p2
, p3
, p0
, pa
);
998 closest_point_segment( p2
, p3
, p1
, pb
);
999 rb_capsule_manifold( p0
, pa
, 0.0f
, r
, &manifold
);
1000 rb_capsule_manifold( p1
, pb
, 1.0f
, r
, &manifold
);
1002 return rb_capsule_manifold_done( rba
, rbb
, &manifold
, buf
);
1006 * Generates up to two contacts; optimised for the most stable manifold
1008 static int rb_capsule_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1010 float h
= rba
->inf
.capsule
.height
,
1011 r
= rba
->inf
.capsule
.radius
;
1014 * Solving this in symetric local space of the cube saves us some time and a
1015 * couple branches when it comes to the quad stage.
1018 v3_add( rbb
->bbx
[0], rbb
->bbx
[1], centroid
);
1019 v3_muls( centroid
, 0.5f
, centroid
);
1022 v3_sub( rbb
->bbx
[0], centroid
, bbx
[0] );
1023 v3_sub( rbb
->bbx
[1], centroid
, bbx
[1] );
1025 v3f pc
, p0w
, p1w
, p0
, p1
;
1026 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+r
, p0w
);
1027 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-r
, p1w
);
1029 m4x3_mulv( rbb
->to_local
, p0w
, p0
);
1030 m4x3_mulv( rbb
->to_local
, p1w
, p1
);
1031 v3_sub( p0
, centroid
, p0
);
1032 v3_sub( p1
, centroid
, p1
);
1033 v3_add( p0
, p1
, pc
);
1034 v3_muls( pc
, 0.5f
, pc
);
1037 * Finding an appropriate quad to collide lines with
1040 v3_div( pc
, bbx
[1], region
);
1043 if( (fabsf(region
[0]) > fabsf(region
[1])) &&
1044 (fabsf(region
[0]) > fabsf(region
[2])) )
1046 float px
= vg_signf(region
[0]) * bbx
[1][0];
1047 v3_copy( (v3f
){ px
, bbx
[0][1], bbx
[0][2] }, quad
[0] );
1048 v3_copy( (v3f
){ px
, bbx
[1][1], bbx
[0][2] }, quad
[1] );
1049 v3_copy( (v3f
){ px
, bbx
[1][1], bbx
[1][2] }, quad
[2] );
1050 v3_copy( (v3f
){ px
, bbx
[0][1], bbx
[1][2] }, quad
[3] );
1052 else if( fabsf(region
[1]) > fabsf(region
[2]) )
1054 float py
= vg_signf(region
[1]) * bbx
[1][1];
1055 v3_copy( (v3f
){ bbx
[0][0], py
, bbx
[0][2] }, quad
[0] );
1056 v3_copy( (v3f
){ bbx
[1][0], py
, bbx
[0][2] }, quad
[1] );
1057 v3_copy( (v3f
){ bbx
[1][0], py
, bbx
[1][2] }, quad
[2] );
1058 v3_copy( (v3f
){ bbx
[0][0], py
, bbx
[1][2] }, quad
[3] );
1062 float pz
= vg_signf(region
[2]) * bbx
[1][2];
1063 v3_copy( (v3f
){ bbx
[0][0], bbx
[0][1], pz
}, quad
[0] );
1064 v3_copy( (v3f
){ bbx
[1][0], bbx
[0][1], pz
}, quad
[1] );
1065 v3_copy( (v3f
){ bbx
[1][0], bbx
[1][1], pz
}, quad
[2] );
1066 v3_copy( (v3f
){ bbx
[0][0], bbx
[1][1], pz
}, quad
[3] );
1069 capsule_manifold manifold
;
1070 rb_capsule_manifold_init( &manifold
);
1073 closest_point_aabb( p0
, bbx
, c0
);
1074 closest_point_aabb( p1
, bbx
, c1
);
1077 v3_sub( c0
, p0
, d0
);
1078 v3_sub( c1
, p1
, d1
);
1079 v3_sub( p1
, p0
, da
);
1086 if( v3_dot( da
, d0
) <= 0.01f
)
1087 rb_capsule_manifold( p0
, c0
, 0.0f
, r
, &manifold
);
1089 if( v3_dot( da
, d1
) >= -0.01f
)
1090 rb_capsule_manifold( p1
, c1
, 1.0f
, r
, &manifold
);
1092 for( int i
=0; i
<4; i
++ )
1099 closest_segment_segment( p0
, p1
, quad
[i0
], quad
[i1
], &ta
, &tb
, ca
, cb
);
1100 rb_capsule_manifold( ca
, cb
, ta
, r
, &manifold
);
1104 * Create final contacts based on line manifold
1106 m3x3_mulv( rbb
->to_world
, manifold
.d0
, manifold
.d0
);
1107 m3x3_mulv( rbb
->to_world
, manifold
.d1
, manifold
.d1
);
1114 for( int i
=0; i
<4; i
++ )
1120 v3_add( quad
[i0
], centroid
, q0
);
1121 v3_add( quad
[i1
], centroid
, q1
);
1123 m4x3_mulv( rbb
->to_world
, q0
, q0
);
1124 m4x3_mulv( rbb
->to_world
, q1
, q1
);
1126 vg_line( q0
, q1
, 0xffffffff );
1130 return rb_capsule_manifold_done( rba
, rbb
, &manifold
, buf
);
1133 static int rb_sphere_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1137 closest_point_obb( rba
->co
, rbb
, co
);
1138 v3_sub( rba
->co
, co
, delta
);
1140 float d2
= v3_length2(delta
),
1141 r
= rba
->inf
.sphere
.radius
;
1150 v3_sub( rba
->co
, rbb
->co
, delta
);
1153 * some extra testing is required to find the best axis to push the
1154 * object back outside the box. Since there isnt a clear seperating
1155 * vector already, especially on really high aspect boxes.
1157 float lx
= v3_dot( rbb
->right
, delta
),
1158 ly
= v3_dot( rbb
->up
, delta
),
1159 lz
= v3_dot( rbb
->forward
, delta
),
1160 px
= rbb
->bbx
[1][0] - fabsf(lx
),
1161 py
= rbb
->bbx
[1][1] - fabsf(ly
),
1162 pz
= rbb
->bbx
[1][2] - fabsf(lz
);
1164 if( px
< py
&& px
< pz
)
1165 v3_muls( rbb
->right
, vg_signf(lx
), ct
->n
);
1167 v3_muls( rbb
->up
, vg_signf(ly
), ct
->n
);
1169 v3_muls( rbb
->forward
, vg_signf(lz
), ct
->n
);
1171 v3_muladds( rba
->co
, ct
->n
, -r
, ct
->co
);
1177 v3_muls( delta
, 1.0f
/d
, ct
->n
);
1179 v3_copy( co
, ct
->co
);
1190 static int rb_sphere_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1193 v3_sub( rba
->co
, rbb
->co
, delta
);
1195 float d2
= v3_length2(delta
),
1196 r
= rba
->inf
.sphere
.radius
+ rbb
->inf
.sphere
.radius
;
1200 float d
= sqrtf(d2
);
1203 v3_muls( delta
, 1.0f
/d
, ct
->n
);
1206 v3_muladds( rba
->co
, ct
->n
,-rba
->inf
.sphere
.radius
, p0
);
1207 v3_muladds( rbb
->co
, ct
->n
, rbb
->inf
.sphere
.radius
, p1
);
1208 v3_add( p0
, p1
, ct
->co
);
1209 v3_muls( ct
->co
, 0.5f
, ct
->co
);
1219 static int rb_sphere_triangle( rigidbody
*rba
, rigidbody
*rbb
,
1220 v3f tri
[3], rb_ct
*buf
)
1224 closest_on_triangle( rba
->co
, tri
, co
);
1225 v3_sub( rba
->co
, co
, delta
);
1227 vg_line( rba
->co
, co
, 0xffff0000 );
1228 vg_line_pt3( rba
->co
, 0.1f
, 0xff00ffff );
1230 float d2
= v3_length2( delta
),
1231 r
= rba
->inf
.sphere
.radius
;
1238 v3_sub( tri
[2], tri
[0], ab
);
1239 v3_sub( tri
[1], tri
[0], ac
);
1240 v3_cross( ac
, ab
, tn
);
1241 v3_copy( tn
, ct
->n
);
1242 v3_normalize( ct
->n
);
1244 float d
= sqrtf(d2
);
1246 v3_copy( co
, ct
->co
);
1256 static int rb_sphere_scene( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1258 scene
*sc
= rbb
->inf
.scene
.pscene
;
1262 int len
= bh_select( &sc
->bhtris
, rba
->bbx_world
, geo
, 128 );
1266 for( int i
=0; i
<len
; i
++ )
1268 u32
*ptri
= &sc
->indices
[ geo
[i
]*3 ];
1270 for( int j
=0; j
<3; j
++ )
1271 v3_copy( sc
->verts
[ptri
[j
]].co
, tri
[j
] );
1273 vg_line(tri
[0],tri
[1],0xff00ff00 );
1274 vg_line(tri
[1],tri
[2],0xff00ff00 );
1275 vg_line(tri
[2],tri
[0],0xff00ff00 );
1277 buf
[count
].element_id
= ptri
[0];
1278 count
+= rb_sphere_triangle( rba
, rbb
, tri
, buf
+count
);
1282 vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
1290 static int rb_box_scene( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1292 scene
*sc
= rbb
->inf
.scene
.pscene
;
1296 int len
= bh_select( &sc
->bhtris
, rba
->bbx_world
, geo
, 128 );
1300 for( int i
=0; i
<len
; i
++ )
1302 u32
*ptri
= &sc
->indices
[ geo
[i
]*3 ];
1304 for( int j
=0; j
<3; j
++ )
1305 v3_copy( sc
->verts
[ptri
[j
]].co
, tri
[j
] );
1307 if( rb_box_triangle_sat( rba
, tri
) )
1309 vg_line(tri
[0],tri
[1],0xff50ff00 );
1310 vg_line(tri
[1],tri
[2],0xff50ff00 );
1311 vg_line(tri
[2],tri
[0],0xff50ff00 );
1315 vg_line(tri
[0],tri
[1],0xff0000ff );
1316 vg_line(tri
[1],tri
[2],0xff0000ff );
1317 vg_line(tri
[2],tri
[0],0xff0000ff );
1323 v3_sub( tri
[1], tri
[0], v0
);
1324 v3_sub( tri
[2], tri
[0], v1
);
1325 v3_cross( v0
, v1
, n
);
1328 /* find best feature */
1329 float best
= v3_dot( rba
->right
, n
);
1332 float cy
= v3_dot( rba
->up
, n
);
1333 if( fabsf(cy
) > fabsf(best
) )
1339 /* TODO: THIS IS WRONG DIRECTION */
1340 float cz
= -v3_dot( rba
->forward
, n
);
1341 if( fabsf(cz
) > fabsf(best
) )
1351 float px
= best
> 0.0f
? rba
->bbx
[0][0]: rba
->bbx
[1][0];
1352 manifold
[0][0] = px
;
1353 manifold
[0][1] = rba
->bbx
[0][1];
1354 manifold
[0][2] = rba
->bbx
[0][2];
1355 manifold
[1][0] = px
;
1356 manifold
[1][1] = rba
->bbx
[1][1];
1357 manifold
[1][2] = rba
->bbx
[0][2];
1358 manifold
[2][0] = px
;
1359 manifold
[2][1] = rba
->bbx
[1][1];
1360 manifold
[2][2] = rba
->bbx
[1][2];
1361 manifold
[3][0] = px
;
1362 manifold
[3][1] = rba
->bbx
[0][1];
1363 manifold
[3][2] = rba
->bbx
[1][2];
1365 else if( axis
== 1 )
1367 float py
= best
> 0.0f
? rba
->bbx
[0][1]: rba
->bbx
[1][1];
1368 manifold
[0][0] = rba
->bbx
[0][0];
1369 manifold
[0][1] = py
;
1370 manifold
[0][2] = rba
->bbx
[0][2];
1371 manifold
[1][0] = rba
->bbx
[1][0];
1372 manifold
[1][1] = py
;
1373 manifold
[1][2] = rba
->bbx
[0][2];
1374 manifold
[2][0] = rba
->bbx
[1][0];
1375 manifold
[2][1] = py
;
1376 manifold
[2][2] = rba
->bbx
[1][2];
1377 manifold
[3][0] = rba
->bbx
[0][0];
1378 manifold
[3][1] = py
;
1379 manifold
[3][2] = rba
->bbx
[1][2];
1383 float pz
= best
> 0.0f
? rba
->bbx
[0][2]: rba
->bbx
[1][2];
1384 manifold
[0][0] = rba
->bbx
[0][0];
1385 manifold
[0][1] = rba
->bbx
[0][1];
1386 manifold
[0][2] = pz
;
1387 manifold
[1][0] = rba
->bbx
[1][0];
1388 manifold
[1][1] = rba
->bbx
[0][1];
1389 manifold
[1][2] = pz
;
1390 manifold
[2][0] = rba
->bbx
[1][0];
1391 manifold
[2][1] = rba
->bbx
[1][1];
1392 manifold
[2][2] = pz
;
1393 manifold
[3][0] = rba
->bbx
[0][0];
1394 manifold
[3][1] = rba
->bbx
[1][1];
1395 manifold
[3][2] = pz
;
1398 for( int j
=0; j
<4; j
++ )
1399 m4x3_mulv( rba
->to_world
, manifold
[j
], manifold
[j
] );
1401 vg_line( manifold
[0], manifold
[1], 0xffffffff );
1402 vg_line( manifold
[1], manifold
[2], 0xffffffff );
1403 vg_line( manifold
[2], manifold
[3], 0xffffffff );
1404 vg_line( manifold
[3], manifold
[0], 0xffffffff );
1406 for( int j
=0; j
<4; j
++ )
1408 rb_ct
*ct
= buf
+count
;
1410 v3_copy( manifold
[j
], ct
->co
);
1411 v3_copy( n
, ct
->n
);
1413 float l0
= v3_dot( tri
[0], n
),
1414 l1
= v3_dot( manifold
[j
], n
);
1416 ct
->p
= (l0
-l1
)*0.5f
;
1431 static int RB_MATRIX_ERROR( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1433 vg_error( "Collision type is unimplemented between types %d and %d\n",
1434 rba
->type
, rbb
->type
);
1439 static int rb_sphere_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1441 return rb_capsule_sphere( rbb
, rba
, buf
);
1444 static int rb_box_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1446 return rb_capsule_box( rbb
, rba
, buf
);
1449 static int rb_box_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1451 return rb_sphere_box( rbb
, rba
, buf
);
1454 static int rb_scene_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1456 return rb_box_scene( rbb
, rba
, buf
);
1459 static int (*rb_jump_table
[4][4])( rigidbody
*a
, rigidbody
*b
, rb_ct
*buf
) =
1461 /* box */ /* Sphere */ /* Capsule */ /* Mesh */
1462 { RB_MATRIX_ERROR
, rb_box_sphere
, rb_box_capsule
, rb_box_scene
},
1463 { rb_sphere_box
, rb_sphere_sphere
, rb_sphere_capsule
, rb_sphere_scene
},
1464 { rb_capsule_box
, rb_capsule_sphere
, rb_capsule_capsule
, RB_MATRIX_ERROR
},
1465 { rb_scene_box
, RB_MATRIX_ERROR
, RB_MATRIX_ERROR
, RB_MATRIX_ERROR
}
1468 static int rb_collide( rigidbody
*rba
, rigidbody
*rbb
)
1470 int (*collider_jump
)(rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1471 = rb_jump_table
[rba
->type
][rbb
->type
];
1474 * 12 is the maximum manifold size we can generate, so we are forced to abort
1475 * potentially checking any more.
1477 if( rb_contact_count
+ 12 > vg_list_size(rb_contact_buffer
) )
1479 vg_warn( "Too many contacts made in global collider buffer (%d of %d\n)",
1480 rb_contact_count
, vg_list_size(rb_contact_buffer
) );
1485 * TODO: Replace this with a more dedicated broad phase pass
1487 if( box_overlap( rba
->bbx_world
, rbb
->bbx_world
) )
1489 int count
= collider_jump( rba
, rbb
, rb_contact_buffer
+rb_contact_count
);
1490 rb_contact_count
+= count
;
1498 * -----------------------------------------------------------------------------
1500 * -----------------------------------------------------------------------------
1503 static void rb_solver_reset(void)
1505 rb_contact_count
= 0;
1508 static rb_ct
*rb_global_ct(void)
1510 return rb_contact_buffer
+ rb_contact_count
;
1514 * Initializing things like tangent vectors
1516 static void rb_presolve_contacts( rb_ct
*buffer
, int len
)
1518 for( int i
=0; i
<len
; i
++ )
1520 rb_ct
*ct
= &buffer
[i
];
1521 ct
->bias
= -0.2f
* k_rb_rate
* vg_minf( 0.0f
, -ct
->p
+k_penetration_slop
);
1522 rb_tangent_basis( ct
->n
, ct
->t
[0], ct
->t
[1] );
1524 ct
->norm_impulse
= 0.0f
;
1525 ct
->tangent_impulse
[0] = 0.0f
;
1526 ct
->tangent_impulse
[1] = 0.0f
;
1528 v3f ra
, rb
, raCn
, rbCn
, raCt
, rbCt
;
1529 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
1530 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
1531 v3_cross( ra
, ct
->n
, raCn
);
1532 v3_cross( rb
, ct
->n
, rbCn
);
1534 /* orient inverse inertia tensors */
1536 m3x3_mulv( ct
->rba
->iIw
, raCn
, raCnI
);
1537 m3x3_mulv( ct
->rbb
->iIw
, rbCn
, rbCnI
);
1539 ct
->normal_mass
= ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
1540 ct
->normal_mass
+= v3_dot( raCn
, raCnI
);
1541 ct
->normal_mass
+= v3_dot( rbCn
, rbCnI
);
1542 ct
->normal_mass
= 1.0f
/ct
->normal_mass
;
1544 for( int j
=0; j
<2; j
++ )
1547 v3_cross( ct
->t
[j
], ra
, raCt
);
1548 v3_cross( ct
->t
[j
], rb
, rbCt
);
1549 m3x3_mulv( ct
->rba
->iIw
, raCt
, raCtI
);
1550 m3x3_mulv( ct
->rbb
->iIw
, rbCt
, rbCtI
);
1552 ct
->tangent_mass
[j
] = ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
1553 ct
->tangent_mass
[j
] += v3_dot( raCt
, raCtI
);
1554 ct
->tangent_mass
[j
] += v3_dot( rbCt
, rbCtI
);
1555 ct
->tangent_mass
[j
] = 1.0f
/ct
->tangent_mass
[j
];
1558 rb_debug_contact( ct
);
1563 * Creates relative contact velocity vector, and offsets between each body
1565 static void rb_rcv( rb_ct
*ct
, v3f rv
, v3f da
, v3f db
)
1567 rigidbody
*rba
= ct
->rba
,
1570 v3_sub( ct
->co
, rba
->co
, da
);
1571 v3_sub( ct
->co
, rbb
->co
, db
);
1574 v3_cross( rba
->w
, da
, rva
);
1575 v3_add( rba
->v
, rva
, rva
);
1576 v3_cross( rbb
->w
, db
, rvb
);
1577 v3_add( rbb
->v
, rvb
, rvb
);
1579 v3_sub( rva
, rvb
, rv
);
1583 * Apply impulse to object
1585 static void rb_linear_impulse( rigidbody
*rb
, v3f delta
, v3f impulse
)
1588 v3_muladds( rb
->v
, impulse
, rb
->inv_mass
, rb
->v
);
1590 /* Angular velocity */
1592 v3_cross( delta
, impulse
, wa
);
1594 m3x3_mulv( rb
->iIw
, wa
, wa
);
1595 v3_add( rb
->w
, wa
, rb
->w
);
1599 * One iteration to solve the contact constraint
1601 static void rb_solve_contacts( rb_ct
*buf
, int len
)
1603 for( int i
=0; i
<len
; i
++ )
1605 struct contact
*ct
= &buf
[i
];
1606 rigidbody
*rb
= ct
->rba
;
1609 rb_rcv( ct
, rv
, da
, db
);
1612 for( int j
=0; j
<2; j
++ )
1614 float f
= k_friction
* ct
->norm_impulse
,
1615 vt
= v3_dot( rv
, ct
->t
[j
] ),
1616 lambda
= ct
->tangent_mass
[j
] * -vt
;
1618 float temp
= ct
->tangent_impulse
[j
];
1619 ct
->tangent_impulse
[j
] = vg_clampf( temp
+ lambda
, -f
, f
);
1620 lambda
= ct
->tangent_impulse
[j
] - temp
;
1623 v3_muls( ct
->t
[j
], lambda
, impulse
);
1624 rb_linear_impulse( ct
->rba
, da
, impulse
);
1626 v3_muls( ct
->t
[j
], -lambda
, impulse
);
1627 rb_linear_impulse( ct
->rbb
, db
, impulse
);
1631 rb_rcv( ct
, rv
, da
, db
);
1632 float vn
= v3_dot( rv
, ct
->n
),
1633 lambda
= ct
->normal_mass
* (-vn
+ ct
->bias
);
1635 float temp
= ct
->norm_impulse
;
1636 ct
->norm_impulse
= vg_maxf( temp
+ lambda
, 0.0f
);
1637 lambda
= ct
->norm_impulse
- temp
;
1640 v3_muls( ct
->n
, lambda
, impulse
);
1641 rb_linear_impulse( ct
->rba
, da
, impulse
);
1643 v3_muls( ct
->n
, -lambda
, impulse
);
1644 rb_linear_impulse( ct
->rbb
, db
, impulse
);
1649 * -----------------------------------------------------------------------------
1651 * -----------------------------------------------------------------------------
1654 static void draw_angle_limit( v3f c
, v3f major
, v3f minor
,
1655 float amin
, float amax
, float measured
,
1660 v3_muls( major
, f
, ay
);
1661 v3_muls( minor
, f
, ax
);
1663 for( int x
=0; x
<16; x
++ )
1665 float t0
= (float)x
/ 16.0f
,
1666 t1
= (float)(x
+1) / 16.0f
,
1667 a0
= vg_lerpf( amin
, amax
, t0
),
1668 a1
= vg_lerpf( amin
, amax
, t1
);
1671 v3_muladds( c
, ay
, cosf(a0
), p0
);
1672 v3_muladds( p0
, ax
, sinf(a0
), p0
);
1673 v3_muladds( c
, ay
, cosf(a1
), p1
);
1674 v3_muladds( p1
, ax
, sinf(a1
), p1
);
1676 vg_line( p0
, p1
, colour
);
1679 vg_line( c
, p0
, colour
);
1681 vg_line( c
, p1
, colour
);
1685 v3_muladds( c
, ay
, cosf(measured
)*1.2f
, p2
);
1686 v3_muladds( p2
, ax
, sinf(measured
)*1.2f
, p2
);
1687 vg_line( c
, p2
, colour
);
1690 static void rb_debug_constraint_limits( rigidbody
*ra
, rigidbody
*rb
, v3f lca
,
1693 v3f ax
, ay
, az
, bx
, by
, bz
;
1694 m3x3_mulv( ra
->to_world
, (v3f
){1.0f
,0.0f
,0.0f
}, ax
);
1695 m3x3_mulv( ra
->to_world
, (v3f
){0.0f
,1.0f
,0.0f
}, ay
);
1696 m3x3_mulv( ra
->to_world
, (v3f
){0.0f
,0.0f
,1.0f
}, az
);
1697 m3x3_mulv( rb
->to_world
, (v3f
){1.0f
,0.0f
,0.0f
}, bx
);
1698 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,1.0f
,0.0f
}, by
);
1699 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,0.0f
,1.0f
}, bz
);
1702 px
[0] = v3_dot( ay
, by
);
1703 px
[1] = v3_dot( az
, by
);
1705 py
[0] = v3_dot( az
, bz
);
1706 py
[1] = v3_dot( ax
, bz
);
1708 pz
[0] = v3_dot( ax
, bx
);
1709 pz
[1] = v3_dot( ay
, bx
);
1711 float r0
= atan2f( px
[1], px
[0] ),
1712 r1
= atan2f( py
[1], py
[0] ),
1713 r2
= atan2f( pz
[1], pz
[0] );
1716 m4x3_mulv( ra
->to_world
, lca
, c
);
1717 draw_angle_limit( c
, ay
, az
, limits
[0][0], limits
[1][0], r0
, 0xff0000ff );
1718 draw_angle_limit( c
, az
, ax
, limits
[0][1], limits
[1][1], r1
, 0xff00ff00 );
1719 draw_angle_limit( c
, ax
, ay
, limits
[0][2], limits
[1][2], r2
, 0xffff0000 );
1722 static void rb_limit_cure( rigidbody
*ra
, rigidbody
*rb
, v3f axis
, float d
)
1726 float avx
= v3_dot( ra
->w
, axis
) - v3_dot( rb
->w
, axis
);
1727 float joint_mass
= rb
->inv_mass
+ ra
->inv_mass
;
1728 joint_mass
= 1.0f
/joint_mass
;
1730 float bias
= (k_limit_bias
* k_rb_rate
) * d
,
1731 lambda
= -(avx
+ bias
) * joint_mass
;
1733 /* Angular velocity */
1735 v3_muls( axis
, lambda
* ra
->inv_mass
, wa
);
1736 v3_muls( axis
, -lambda
* rb
->inv_mass
, wb
);
1738 v3_add( ra
->w
, wa
, ra
->w
);
1739 v3_add( rb
->w
, wb
, rb
->w
);
1743 static void rb_constraint_limits( rigidbody
*ra
, v3f lca
,
1744 rigidbody
*rb
, v3f lcb
, v3f limits
[2] )
1746 /* TODO: Code dupe remover */
1747 v3f ax
, ay
, az
, bx
, by
, bz
;
1748 m3x3_mulv( ra
->to_world
, (v3f
){1.0f
,0.0f
,0.0f
}, ax
);
1749 m3x3_mulv( ra
->to_world
, (v3f
){0.0f
,1.0f
,0.0f
}, ay
);
1750 m3x3_mulv( ra
->to_world
, (v3f
){0.0f
,0.0f
,1.0f
}, az
);
1751 m3x3_mulv( rb
->to_world
, (v3f
){1.0f
,0.0f
,0.0f
}, bx
);
1752 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,1.0f
,0.0f
}, by
);
1753 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,0.0f
,1.0f
}, bz
);
1756 px
[0] = v3_dot( ay
, by
);
1757 px
[1] = v3_dot( az
, by
);
1759 py
[0] = v3_dot( az
, bz
);
1760 py
[1] = v3_dot( ax
, bz
);
1762 pz
[0] = v3_dot( ax
, bx
);
1763 pz
[1] = v3_dot( ay
, bx
);
1765 float r0
= atan2f( px
[1], px
[0] ),
1766 r1
= atan2f( py
[1], py
[0] ),
1767 r2
= atan2f( pz
[1], pz
[0] );
1769 /* calculate angle deltas */
1770 float dx
= 0.0f
, dy
= 0.0f
, dz
= 0.0f
;
1772 if( r0
< limits
[0][0] ) dx
= limits
[0][0] - r0
;
1773 if( r0
> limits
[1][0] ) dx
= limits
[1][0] - r0
;
1774 if( r1
< limits
[0][1] ) dy
= limits
[0][1] - r1
;
1775 if( r1
> limits
[1][1] ) dy
= limits
[1][1] - r1
;
1776 if( r2
< limits
[0][2] ) dz
= limits
[0][2] - r2
;
1777 if( r2
> limits
[1][2] ) dz
= limits
[1][2] - r2
;
1780 m3x3_mulv( ra
->to_world
, lca
, wca
);
1781 m3x3_mulv( rb
->to_world
, lcb
, wcb
);
1783 rb_limit_cure( ra
, rb
, ax
, dx
);
1784 rb_limit_cure( ra
, rb
, ay
, dy
);
1785 rb_limit_cure( ra
, rb
, az
, dz
);
1788 static void rb_debug_constraint_position( rigidbody
*ra
, v3f lca
,
1789 rigidbody
*rb
, v3f lcb
)
1792 m3x3_mulv( ra
->to_world
, lca
, wca
);
1793 m3x3_mulv( rb
->to_world
, lcb
, wcb
);
1796 v3_add( wca
, ra
->co
, p0
);
1797 v3_add( wcb
, rb
->co
, p1
);
1798 vg_line_pt3( p0
, 0.005f
, 0xffffff00 );
1799 vg_line_pt3( p1
, 0.005f
, 0xffffff00 );
1800 vg_line( p0
, p1
, 0xffffff00 );
1803 static void rb_constraint_position( rigidbody
*ra
, v3f lca
,
1804 rigidbody
*rb
, v3f lcb
)
1806 /* C = (COa + Ra*LCa) - (COb + Rb*LCb) = 0 */
1808 m3x3_mulv( ra
->to_world
, lca
, wca
);
1809 m3x3_mulv( rb
->to_world
, lcb
, wcb
);
1812 v3_sub( ra
->v
, rb
->v
, rcv
);
1815 v3_cross( ra
->w
, wca
, rcv_Ra
);
1816 v3_cross( rb
->w
, wcb
, rcv_Rb
);
1817 v3_add( rcv_Ra
, rcv
, rcv
);
1818 v3_sub( rcv
, rcv_Rb
, rcv
);
1822 v3_add( wca
, ra
->co
, p0
);
1823 v3_add( wcb
, rb
->co
, p1
);
1824 v3_sub( p1
, p0
, delta
);
1826 float dist2
= v3_length2( delta
);
1828 if( dist2
> 0.00001f
)
1830 float dist
= sqrtf(dist2
);
1831 v3_muls( delta
, 1.0f
/dist
, delta
);
1833 float joint_mass
= rb
->inv_mass
+ ra
->inv_mass
;
1835 v3f raCn
, rbCn
, raCt
, rbCt
;
1836 v3_cross( wca
, delta
, raCn
);
1837 v3_cross( wcb
, delta
, rbCn
);
1839 /* orient inverse inertia tensors */
1841 m3x3_mulv( ra
->iIw
, raCn
, raCnI
);
1842 m3x3_mulv( rb
->iIw
, rbCn
, rbCnI
);
1843 joint_mass
+= v3_dot( raCn
, raCnI
);
1844 joint_mass
+= v3_dot( rbCn
, rbCnI
);
1845 joint_mass
= 1.0f
/joint_mass
;
1847 float vd
= v3_dot( rcv
, delta
),
1848 bias
= -(k_joint_bias
* k_rb_rate
) * dist
,
1849 lambda
= -(vd
+ bias
) * joint_mass
;
1852 v3_muls( delta
, lambda
, impulse
);
1853 rb_linear_impulse( ra
, wca
, impulse
);
1854 v3_muls( delta
, -lambda
, impulse
);
1855 rb_linear_impulse( rb
, wcb
, impulse
);
1858 v3_muladds( ra
->co
, delta
, dist
* k_joint_correction
, ra
->co
);
1859 v3_muladds( rb
->co
, delta
, -dist
* k_joint_correction
, rb
->co
);
1867 static void rb_effect_simple_bouyency( rigidbody
*ra
, v4f plane
,
1868 float amt
, float drag
)
1871 float depth
= v3_dot( plane
, ra
->co
) - plane
[3],
1872 lambda
= vg_clampf( -depth
, 0.0f
, 1.0f
) * amt
;
1874 v3_muladds( ra
->v
, plane
, lambda
* ktimestep
, ra
->v
);
1877 v3_muls( ra
->v
, 1.0f
-(drag
*ktimestep
), ra
->v
);
1881 * -----------------------------------------------------------------------------
1882 * BVH implementation, this is ONLY for static rigidbodies, its to slow for
1884 * -----------------------------------------------------------------------------
1887 static void rb_bh_expand_bound( void *user
, boxf bound
, u32 item_index
)
1889 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
1890 box_concat( bound
, rb
->bbx_world
);
1893 static float rb_bh_centroid( void *user
, u32 item_index
, int axis
)
1895 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
1896 return (rb
->bbx_world
[axis
][0] + rb
->bbx_world
[1][axis
]) * 0.5f
;
1899 static void rb_bh_swap( void *user
, u32 ia
, u32 ib
)
1901 rigidbody temp
, *rba
, *rbb
;
1902 rba
= &((rigidbody
*)user
)[ ia
];
1903 rbb
= &((rigidbody
*)user
)[ ib
];
1910 static void rb_bh_debug( void *user
, u32 item_index
)
1912 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
1913 rb_debug( rb
, 0xff00ffff );
1916 static bh_system bh_system_rigidbodies
=
1918 .expand_bound
= rb_bh_expand_bound
,
1919 .item_centroid
= rb_bh_centroid
,
1920 .item_swap
= rb_bh_swap
,
1921 .item_debug
= rb_bh_debug
,
1925 #endif /* RIGIDBODY_H */