2 * Copyright (C) 2021-2022 Mt.ZERO Software, Harry Godden - All Rights Reserved
6 * Resources: Box2D - Erin Catto
14 VG_STATIC
void rb_tangent_basis( v3f n
, v3f tx
, v3f ty
);
15 VG_STATIC bh_system bh_system_rigidbodies
;
21 * -----------------------------------------------------------------------------
23 * -----------------------------------------------------------------------------
27 k_rb_rate
= (1.0/VG_TIMESTEP_FIXED
),
28 k_rb_delta
= (1.0/k_rb_rate
),
30 k_damp_linear
= 0.05f
, /* scale velocity 1/(1+x) */
31 k_damp_angular
= 0.1f
, /* scale angular 1/(1+x) */
33 k_joint_bias
= 0.08f
, /* positional joints */
34 k_joint_correction
= 0.01f
,
35 k_penetration_slop
= 0.01f
,
36 k_inertia_scale
= 4.0f
;
39 * -----------------------------------------------------------------------------
40 * structure definitions
41 * -----------------------------------------------------------------------------
44 typedef struct rigidbody rigidbody
;
45 typedef struct contact rb_ct
;
55 k_rb_shape_sphere
= 1,
56 k_rb_shape_capsule
= 2,
83 v3f right
, up
, forward
;
90 /* inertia model and inverse world tensor */
94 m4x3f to_world
, to_local
;
97 VG_STATIC
struct contact
102 float p
, bias
, norm_impulse
, tangent_impulse
[2],
103 normal_mass
, tangent_mass
[2];
108 rb_contact_buffer
[256];
109 VG_STATIC
int rb_contact_count
= 0;
112 * -----------------------------------------------------------------------------
114 * -----------------------------------------------------------------------------
117 VG_STATIC
float sphere_volume( float radius
)
119 float r3
= radius
*radius
*radius
;
120 return (4.0f
/3.0f
) * VG_PIf
* r3
;
123 VG_STATIC
void rb_tangent_basis( v3f n
, v3f tx
, v3f ty
)
125 /* Compute tangent basis (box2d) */
126 if( fabsf( n
[0] ) >= 0.57735027f
)
140 v3_cross( n
, tx
, ty
);
144 * -----------------------------------------------------------------------------
146 * -----------------------------------------------------------------------------
149 VG_STATIC
void rb_debug_contact( rb_ct
*ct
)
154 v3_muladds( ct
->co
, ct
->n
, 0.1f
, p1
);
155 vg_line_pt3( ct
->co
, 0.025f
, 0xff0000ff );
156 vg_line( ct
->co
, p1
, 0xffffffff );
160 VG_STATIC
void debug_sphere( m4x3f m
, float radius
, u32 colour
)
162 v3f ly
= { 0.0f
, 0.0f
, radius
},
163 lx
= { 0.0f
, radius
, 0.0f
},
164 lz
= { 0.0f
, 0.0f
, radius
};
166 for( int i
=0; i
<16; i
++ )
168 float t
= ((float)(i
+1) * (1.0f
/16.0f
)) * VG_PIf
* 2.0f
,
172 v3f py
= { s
*radius
, 0.0f
, c
*radius
},
173 px
= { s
*radius
, c
*radius
, 0.0f
},
174 pz
= { 0.0f
, s
*radius
, c
*radius
};
176 v3f p0
, p1
, p2
, p3
, p4
, p5
;
177 m4x3_mulv( m
, py
, p0
);
178 m4x3_mulv( m
, ly
, p1
);
179 m4x3_mulv( m
, px
, p2
);
180 m4x3_mulv( m
, lx
, p3
);
181 m4x3_mulv( m
, pz
, p4
);
182 m4x3_mulv( m
, lz
, p5
);
184 vg_line( p0
, p1
, colour
== 0x00? 0xff00ff00: colour
);
185 vg_line( p2
, p3
, colour
== 0x00? 0xff0000ff: colour
);
186 vg_line( p4
, p5
, colour
== 0x00? 0xffff0000: colour
);
194 VG_STATIC
void debug_capsule( m4x3f m
, float radius
, float h
, u32 colour
)
196 v3f ly
= { 0.0f
, 0.0f
, radius
},
197 lx
= { 0.0f
, radius
, 0.0f
},
198 lz
= { 0.0f
, 0.0f
, radius
};
200 float s0
= sinf(0.0f
)*radius
,
201 c0
= cosf(0.0f
)*radius
;
203 v3f p0
, p1
, up
, right
, forward
;
204 m3x3_mulv( m
, (v3f
){0.0f
,1.0f
,0.0f
}, up
);
205 m3x3_mulv( m
, (v3f
){1.0f
,0.0f
,0.0f
}, right
);
206 m3x3_mulv( m
, (v3f
){0.0f
,0.0f
,-1.0f
}, forward
);
207 v3_muladds( m
[3], up
, -h
*0.5f
+radius
, p0
);
208 v3_muladds( m
[3], up
, h
*0.5f
-radius
, p1
);
211 v3_muladds( p0
, right
, radius
, a0
);
212 v3_muladds( p1
, right
, radius
, a1
);
213 v3_muladds( p0
, forward
, radius
, b0
);
214 v3_muladds( p1
, forward
, radius
, b1
);
215 vg_line( a0
, a1
, colour
);
216 vg_line( b0
, b1
, colour
);
218 v3_muladds( p0
, right
, -radius
, a0
);
219 v3_muladds( p1
, right
, -radius
, a1
);
220 v3_muladds( p0
, forward
, -radius
, b0
);
221 v3_muladds( p1
, forward
, -radius
, b1
);
222 vg_line( a0
, a1
, colour
);
223 vg_line( b0
, b1
, colour
);
225 for( int i
=0; i
<16; i
++ )
227 float t
= ((float)(i
+1) * (1.0f
/16.0f
)) * VG_PIf
* 2.0f
,
231 v3f e0
= { s0
, 0.0f
, c0
},
232 e1
= { s1
, 0.0f
, c1
},
233 e2
= { s0
, c0
, 0.0f
},
234 e3
= { s1
, c1
, 0.0f
},
235 e4
= { 0.0f
, c0
, s0
},
236 e5
= { 0.0f
, c1
, s1
};
238 m3x3_mulv( m
, e0
, e0
);
239 m3x3_mulv( m
, e1
, e1
);
240 m3x3_mulv( m
, e2
, e2
);
241 m3x3_mulv( m
, e3
, e3
);
242 m3x3_mulv( m
, e4
, e4
);
243 m3x3_mulv( m
, e5
, e5
);
245 v3_add( p0
, e0
, a0
);
246 v3_add( p0
, e1
, a1
);
247 v3_add( p1
, e0
, b0
);
248 v3_add( p1
, e1
, b1
);
250 vg_line( a0
, a1
, colour
);
251 vg_line( b0
, b1
, colour
);
255 v3_add( p0
, e2
, a0
);
256 v3_add( p0
, e3
, a1
);
257 v3_add( p0
, e4
, b0
);
258 v3_add( p0
, e5
, b1
);
262 v3_add( p1
, e2
, a0
);
263 v3_add( p1
, e3
, a1
);
264 v3_add( p1
, e4
, b0
);
265 v3_add( p1
, e5
, b1
);
268 vg_line( a0
, a1
, colour
);
269 vg_line( b0
, b1
, colour
);
276 VG_STATIC
void rb_debug( rigidbody
*rb
, u32 colour
)
278 if( rb
->type
== k_rb_shape_box
)
281 vg_line_boxf_transformed( rb
->to_world
, rb
->bbx
, colour
);
283 else if( rb
->type
== k_rb_shape_sphere
)
285 debug_sphere( rb
->to_world
, rb
->inf
.sphere
.radius
, colour
);
287 else if( rb
->type
== k_rb_shape_capsule
)
290 float h
= rb
->inf
.capsule
.height
,
291 r
= rb
->inf
.capsule
.radius
;
293 debug_capsule( rb
->to_world
, r
, h
, colour
);
295 else if( rb
->type
== k_rb_shape_scene
)
297 vg_line_boxf( rb
->bbx
, colour
);
302 * -----------------------------------------------------------------------------
304 * -----------------------------------------------------------------------------
308 * Update world space bounding box based on local one
310 VG_STATIC
void rb_update_bounds( rigidbody
*rb
)
312 box_copy( rb
->bbx
, rb
->bbx_world
);
313 m4x3_transform_aabb( rb
->to_world
, rb
->bbx_world
);
317 * Commit transform to rigidbody. Updates matrices
319 VG_STATIC
void rb_update_transform( rigidbody
*rb
)
321 q_normalize( rb
->q
);
322 q_m3x3( rb
->q
, rb
->to_world
);
323 v3_copy( rb
->co
, rb
->to_world
[3] );
325 m4x3_invert_affine( rb
->to_world
, rb
->to_local
);
327 m3x3_mulv( rb
->to_world
, (v3f
){1.0f
,0.0f
, 0.0f
}, rb
->right
);
328 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,1.0f
, 0.0f
}, rb
->up
);
329 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,0.0f
,-1.0f
}, rb
->forward
);
331 m3x3_mul( rb
->iI
, rb
->to_local
, rb
->iIw
);
332 m3x3_mul( rb
->to_world
, rb
->iIw
, rb
->iIw
);
334 rb_update_bounds( rb
);
338 * Extrapolate rigidbody into a transform based on vg accumulator.
339 * Useful for rendering
341 VG_STATIC
void rb_extrapolate_transform( rigidbody
*rb
, m4x3f transform
)
343 float substep
= vg_clampf( vg
.accumulator
/ k_rb_delta
, 0.0f
, 1.0f
);
348 v3_muladds( rb
->co
, rb
->v
, k_rb_delta
*substep
, co
);
350 if( v3_length2( rb
->w
) > 0.0f
)
354 v3_copy( rb
->w
, axis
);
356 float mag
= v3_length( axis
);
357 v3_divs( axis
, mag
, axis
);
358 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
*substep
);
359 q_mul( rotation
, rb
->q
, q
);
367 q_m3x3( q
, transform
);
368 v3_copy( co
, transform
[3] );
372 * Initialize rigidbody and calculate masses, inertia
374 VG_STATIC
void rb_init( rigidbody
*rb
)
378 if( rb
->type
== k_rb_shape_box
)
381 v3_sub( rb
->bbx
[1], rb
->bbx
[0], dims
);
382 volume
= dims
[0]*dims
[1]*dims
[2];
384 else if( rb
->type
== k_rb_shape_sphere
)
386 volume
= sphere_volume( rb
->inf
.sphere
.radius
);
387 v3_fill( rb
->bbx
[0], -rb
->inf
.sphere
.radius
);
388 v3_fill( rb
->bbx
[1], rb
->inf
.sphere
.radius
);
390 else if( rb
->type
== k_rb_shape_capsule
)
392 float r
= rb
->inf
.capsule
.radius
,
393 h
= rb
->inf
.capsule
.height
;
394 volume
= sphere_volume( r
) + VG_PIf
* r
*r
* (h
- r
*2.0f
);
396 v3_fill( rb
->bbx
[0], -rb
->inf
.sphere
.radius
);
397 v3_fill( rb
->bbx
[1], rb
->inf
.sphere
.radius
);
401 else if( rb
->type
== k_rb_shape_scene
)
404 box_copy( rb
->inf
.scene
.bh_scene
->nodes
[0].bbx
, rb
->bbx
);
415 float mass
= 2.0f
*volume
;
416 rb
->inv_mass
= 1.0f
/mass
;
419 v3_sub( rb
->bbx
[1], rb
->bbx
[0], extent
);
420 v3_muls( extent
, 0.5f
, extent
);
422 /* local intertia tensor */
423 float scale
= k_inertia_scale
;
424 float ex2
= scale
*extent
[0]*extent
[0],
425 ey2
= scale
*extent
[1]*extent
[1],
426 ez2
= scale
*extent
[2]*extent
[2];
428 rb
->I
[0] = ((1.0f
/12.0f
) * mass
* (ey2
+ez2
));
429 rb
->I
[1] = ((1.0f
/12.0f
) * mass
* (ex2
+ez2
));
430 rb
->I
[2] = ((1.0f
/12.0f
) * mass
* (ex2
+ey2
));
432 m3x3_identity( rb
->iI
);
433 rb
->iI
[0][0] = rb
->I
[0];
434 rb
->iI
[1][1] = rb
->I
[1];
435 rb
->iI
[2][2] = rb
->I
[2];
436 m3x3_inv( rb
->iI
, rb
->iI
);
442 rb_update_transform( rb
);
445 VG_STATIC
void rb_iter( rigidbody
*rb
)
447 v3f gravity
= { 0.0f
, -9.8f
, 0.0f
};
448 v3_muladds( rb
->v
, gravity
, k_rb_delta
, rb
->v
);
450 /* intergrate velocity */
451 v3_muladds( rb
->co
, rb
->v
, k_rb_delta
, rb
->co
);
452 v3_lerp( rb
->w
, (v3f
){0.0f
,0.0f
,0.0f
}, 0.0025f
, rb
->w
);
454 /* inegrate inertia */
455 if( v3_length2( rb
->w
) > 0.0f
)
459 v3_copy( rb
->w
, axis
);
461 float mag
= v3_length( axis
);
462 v3_divs( axis
, mag
, axis
);
463 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
);
464 q_mul( rotation
, rb
->q
, rb
->q
);
468 v3_muls( rb
->v
, 1.0f
/(1.0f
+k_rb_delta
*k_damp_linear
), rb
->v
);
469 v3_muls( rb
->w
, 1.0f
/(1.0f
+k_rb_delta
*k_damp_angular
), rb
->w
);
473 * -----------------------------------------------------------------------------
474 * Closest point functions
475 * -----------------------------------------------------------------------------
479 * These closest point tests were learned from Real-Time Collision Detection by
482 VG_STATIC
float closest_segment_segment( v3f p1
, v3f q1
, v3f p2
, v3f q2
,
483 float *s
, float *t
, v3f c1
, v3f c2
)
486 v3_sub( q1
, p1
, d1
);
487 v3_sub( q2
, p2
, d2
);
490 float a
= v3_length2( d1
),
491 e
= v3_length2( d2
),
494 const float kEpsilon
= 0.0001f
;
496 if( a
<= kEpsilon
&& e
<= kEpsilon
)
504 v3_sub( c1
, c2
, v0
);
506 return v3_length2( v0
);
512 *t
= vg_clampf( f
/ e
, 0.0f
, 1.0f
);
516 float c
= v3_dot( d1
, r
);
520 *s
= vg_clampf( -c
/ a
, 0.0f
, 1.0f
);
524 float b
= v3_dot(d1
,d2
),
529 *s
= vg_clampf((b
*f
- c
*e
)/d
, 0.0f
, 1.0f
);
541 *s
= vg_clampf( -c
/ a
, 0.0f
, 1.0f
);
546 *s
= vg_clampf((b
-c
)/a
,0.0f
,1.0f
);
551 v3_muladds( p1
, d1
, *s
, c1
);
552 v3_muladds( p2
, d2
, *t
, c2
);
555 v3_sub( c1
, c2
, v0
);
556 return v3_length2( v0
);
559 VG_STATIC
void closest_point_aabb( v3f p
, boxf box
, v3f dest
)
561 v3_maxv( p
, box
[0], dest
);
562 v3_minv( dest
, box
[1], dest
);
565 VG_STATIC
void closest_point_obb( v3f p
, rigidbody
*rb
, v3f dest
)
568 m4x3_mulv( rb
->to_local
, p
, local
);
569 closest_point_aabb( local
, rb
->bbx
, local
);
570 m4x3_mulv( rb
->to_world
, local
, dest
);
573 VG_STATIC
float closest_point_segment( v3f a
, v3f b
, v3f point
, v3f dest
)
577 v3_sub( point
, a
, v1
);
579 float t
= v3_dot( v1
, v0
) / v3_length2(v0
);
580 t
= vg_clampf(t
,0.0f
,1.0f
);
581 v3_muladds( a
, v0
, t
, dest
);
585 VG_STATIC
void closest_on_triangle( v3f p
, v3f tri
[3], v3f dest
)
590 /* Region outside A */
591 v3_sub( tri
[1], tri
[0], ab
);
592 v3_sub( tri
[2], tri
[0], ac
);
593 v3_sub( p
, tri
[0], ap
);
597 if( d1
<= 0.0f
&& d2
<= 0.0f
)
599 v3_copy( tri
[0], dest
);
600 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
604 /* Region outside B */
608 v3_sub( p
, tri
[1], bp
);
609 d3
= v3_dot( ab
, bp
);
610 d4
= v3_dot( ac
, bp
);
612 if( d3
>= 0.0f
&& d4
<= d3
)
614 v3_copy( tri
[1], dest
);
615 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
619 /* Edge region of AB */
620 float vc
= d1
*d4
- d3
*d2
;
621 if( vc
<= 0.0f
&& d1
>= 0.0f
&& d3
<= 0.0f
)
623 float v
= d1
/ (d1
-d3
);
624 v3_muladds( tri
[0], ab
, v
, dest
);
625 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
629 /* Region outside C */
632 v3_sub( p
, tri
[2], cp
);
636 if( d6
>= 0.0f
&& d5
<= d6
)
638 v3_copy( tri
[2], dest
);
639 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
644 float vb
= d5
*d2
- d1
*d6
;
645 if( vb
<= 0.0f
&& d2
>= 0.0f
&& d6
<= 0.0f
)
647 float w
= d2
/ (d2
-d6
);
648 v3_muladds( tri
[0], ac
, w
, dest
);
649 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
654 float va
= d3
*d6
- d5
*d4
;
655 if( va
<= 0.0f
&& (d4
-d3
) >= 0.0f
&& (d5
-d6
) >= 0.0f
)
657 float w
= (d4
-d3
) / ((d4
-d3
) + (d5
-d6
));
659 v3_sub( tri
[2], tri
[1], bc
);
660 v3_muladds( tri
[1], bc
, w
, dest
);
661 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
665 /* P inside region, Q via barycentric coordinates uvw */
666 float d
= 1.0f
/(va
+vb
+vc
),
670 v3_muladds( tri
[0], ab
, v
, dest
);
671 v3_muladds( dest
, ac
, w
, dest
);
674 VG_STATIC
void closest_on_triangle_1( v3f p
, v3f tri
[3], v3f dest
)
679 /* Region outside A */
680 v3_sub( tri
[1], tri
[0], ab
);
681 v3_sub( tri
[2], tri
[0], ac
);
682 v3_sub( p
, tri
[0], ap
);
686 if( d1
<= 0.0f
&& d2
<= 0.0f
)
688 v3_copy( tri
[0], dest
);
692 /* Region outside B */
696 v3_sub( p
, tri
[1], bp
);
697 d3
= v3_dot( ab
, bp
);
698 d4
= v3_dot( ac
, bp
);
700 if( d3
>= 0.0f
&& d4
<= d3
)
702 v3_copy( tri
[1], dest
);
706 /* Edge region of AB */
707 float vc
= d1
*d4
- d3
*d2
;
708 if( vc
<= 0.0f
&& d1
>= 0.0f
&& d3
<= 0.0f
)
710 float v
= d1
/ (d1
-d3
);
711 v3_muladds( tri
[0], ab
, v
, dest
);
715 /* Region outside C */
718 v3_sub( p
, tri
[2], cp
);
722 if( d6
>= 0.0f
&& d5
<= d6
)
724 v3_copy( tri
[2], dest
);
729 float vb
= d5
*d2
- d1
*d6
;
730 if( vb
<= 0.0f
&& d2
>= 0.0f
&& d6
<= 0.0f
)
732 float w
= d2
/ (d2
-d6
);
733 v3_muladds( tri
[0], ac
, w
, dest
);
738 float va
= d3
*d6
- d5
*d4
;
739 if( va
<= 0.0f
&& (d4
-d3
) >= 0.0f
&& (d5
-d6
) >= 0.0f
)
741 float w
= (d4
-d3
) / ((d4
-d3
) + (d5
-d6
));
743 v3_sub( tri
[2], tri
[1], bc
);
744 v3_muladds( tri
[1], bc
, w
, dest
);
748 /* P inside region, Q via barycentric coordinates uvw */
749 float d
= 1.0f
/(va
+vb
+vc
),
753 v3_muladds( tri
[0], ab
, v
, dest
);
754 v3_muladds( dest
, ac
, w
, dest
);
758 * -----------------------------------------------------------------------------
759 * Boolean shape overlap functions
760 * -----------------------------------------------------------------------------
764 * Project AABB, and triangle interval onto axis to check if they overlap
766 VG_STATIC
int rb_box_triangle_interval( v3f extent
, v3f axis
, v3f tri
[3] )
770 r
= extent
[0] * fabsf(axis
[0]) +
771 extent
[1] * fabsf(axis
[1]) +
772 extent
[2] * fabsf(axis
[2]),
774 p0
= v3_dot( axis
, tri
[0] ),
775 p1
= v3_dot( axis
, tri
[1] ),
776 p2
= v3_dot( axis
, tri
[2] ),
778 e
= vg_maxf(-vg_maxf(p0
,vg_maxf(p1
,p2
)), vg_minf(p0
,vg_minf(p1
,p2
)));
780 if( e
> r
) return 0;
785 * Seperating axis test box vs triangle
787 VG_STATIC
int rb_box_triangle_sat( rigidbody
*rba
, v3f tri_src
[3] )
792 v3_sub( rba
->bbx
[1], rba
->bbx
[0], extent
);
793 v3_muls( extent
, 0.5f
, extent
);
794 v3_add( rba
->bbx
[0], extent
, c
);
796 for( int i
=0; i
<3; i
++ )
798 m4x3_mulv( rba
->to_local
, tri_src
[i
], tri
[i
] );
799 v3_sub( tri
[i
], c
, tri
[i
] );
803 if(!rb_box_triangle_interval( extent
, (v3f
){1.0f
,0.0f
,0.0f
}, tri
)) return 0;
804 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,1.0f
,0.0f
}, tri
)) return 0;
805 if(!rb_box_triangle_interval( extent
, (v3f
){0.0f
,0.0f
,1.0f
}, tri
)) return 0;
807 v3f v0
,v1
,v2
,n
, e0
,e1
,e2
;
808 v3_sub( tri
[1], tri
[0], v0
);
809 v3_sub( tri
[2], tri
[0], v1
);
810 v3_sub( tri
[2], tri
[1], v2
);
814 v3_cross( v0
, v1
, n
);
815 v3_cross( v0
, n
, e0
);
816 v3_cross( n
, v1
, e1
);
817 v3_cross( v2
, n
, e2
);
820 if(!rb_box_triangle_interval( extent
, n
, tri
)) return 0;
823 v3_cross( e0
, (v3f
){1.0f
,0.0f
,0.0f
}, axis
[0] );
824 v3_cross( e0
, (v3f
){0.0f
,1.0f
,0.0f
}, axis
[1] );
825 v3_cross( e0
, (v3f
){0.0f
,0.0f
,1.0f
}, axis
[2] );
826 v3_cross( e1
, (v3f
){1.0f
,0.0f
,0.0f
}, axis
[3] );
827 v3_cross( e1
, (v3f
){0.0f
,1.0f
,0.0f
}, axis
[4] );
828 v3_cross( e1
, (v3f
){0.0f
,0.0f
,1.0f
}, axis
[5] );
829 v3_cross( e2
, (v3f
){1.0f
,0.0f
,0.0f
}, axis
[6] );
830 v3_cross( e2
, (v3f
){0.0f
,1.0f
,0.0f
}, axis
[7] );
831 v3_cross( e2
, (v3f
){0.0f
,0.0f
,1.0f
}, axis
[8] );
833 for( int i
=0; i
<9; i
++ )
834 if(!rb_box_triangle_interval( extent
, axis
[i
], tri
)) return 0;
840 * -----------------------------------------------------------------------------
842 * -----------------------------------------------------------------------------
845 VG_STATIC
int rb_manifold_apply_filtered( rb_ct
*man
, int len
)
849 for( int i
=0; i
<len
; i
++ )
863 * Resolve overlapping pairs
865 VG_STATIC
void rb_manifold_filter_pairs( rb_ct
*man
, int len
, float r
)
867 for( int i
=0; i
<len
-1; i
++ )
872 if( ci
->disabled
) continue;
874 for( int j
=i
+1; j
<len
; j
++ )
878 if( cj
->disabled
) continue;
880 if( v3_dist2( ci
->co
, cj
->co
) < r
*r
)
883 v3_add( cj
->n
, ci
->n
, ci
->n
);
891 float n
= 1.0f
/((float)similar
+1.0f
);
892 v3_muls( ci
->n
, n
, ci
->n
);
895 if( v3_length2(ci
->n
) < 0.1f
*0.1f
)
898 v3_normalize( ci
->n
);
904 * Remove contacts that are facing away from A
906 VG_STATIC
void rb_manifold_filter_backface( rb_ct
*man
, int len
)
908 for( int i
=0; i
<len
; i
++ )
911 if( ct
->disabled
) continue;
914 v3_sub( ct
->co
, ct
->rba
->co
, delta
);
916 if( v3_dot( delta
, ct
->n
) > -0.001f
)
922 * Filter out duplicate coplanar results. Good for spheres.
924 VG_STATIC
void rb_manifold_filter_coplanar( rb_ct
*man
, int len
, float w
)
926 for( int i
=0; i
<len
-1; i
++ )
929 if( ci
->disabled
) continue;
931 float d1
= v3_dot( ci
->co
, ci
->n
);
933 for( int j
=i
+1; j
<len
; j
++ )
936 if( cj
->disabled
) continue;
938 float d2
= v3_dot( cj
->co
, ci
->n
),
941 if( fabsf( d
) <= w
)
948 * -----------------------------------------------------------------------------
950 * -----------------------------------------------------------------------------
956 * These do not automatically allocate contacts, an appropriately sized
957 * buffer must be supplied. The function returns the size of the manifold
958 * which was generated.
960 * The values set on the contacts are: n, co, p, rba, rbb
964 * By collecting the minimum(time) and maximum(time) pairs of points, we
965 * build a reduced and stable exact manifold.
968 * rx: minimum distance of these points
969 * dx: the delta between the two points
971 * pairs will only ammend these if they are creating a collision
973 typedef struct capsule_manifold capsule_manifold
;
974 struct capsule_manifold
982 * Expand a line manifold with a new pair. t value is the time along segment
983 * on the oriented object which created this pair.
985 VG_STATIC
void rb_capsule_manifold( v3f pa
, v3f pb
, float t
, float r
,
986 capsule_manifold
*manifold
)
989 v3_sub( pa
, pb
, delta
);
991 if( v3_length2(delta
) < r
*r
)
993 if( t
< manifold
->t0
)
995 v3_copy( delta
, manifold
->d0
);
1000 if( t
> manifold
->t1
)
1002 v3_copy( delta
, manifold
->d1
);
1009 VG_STATIC
void rb_capsule_manifold_init( capsule_manifold
*manifold
)
1011 manifold
->t0
= INFINITY
;
1012 manifold
->t1
= -INFINITY
;
1015 VG_STATIC
int rb_capsule_manifold_done( rigidbody
*rba
, rigidbody
*rbb
,
1016 capsule_manifold
*manifold
, rb_ct
*buf
)
1018 float h
= rba
->inf
.capsule
.height
,
1019 ra
= rba
->inf
.capsule
.radius
;
1022 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+ra
, p0
);
1023 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-ra
, p1
);
1026 if( manifold
->t0
<= 1.0f
)
1031 v3_muls( p0
, 1.0f
-manifold
->t0
, pa
);
1032 v3_muladds( pa
, p1
, manifold
->t0
, pa
);
1034 float d
= v3_length( manifold
->d0
);
1035 v3_muls( manifold
->d0
, 1.0f
/d
, ct
->n
);
1036 v3_muladds( pa
, ct
->n
, -ra
, ct
->co
);
1038 ct
->p
= manifold
->r0
- d
;
1046 if( (manifold
->t1
>= 0.0f
) && (manifold
->t0
!= manifold
->t1
) )
1048 rb_ct
*ct
= buf
+count
;
1051 v3_muls( p0
, 1.0f
-manifold
->t1
, pa
);
1052 v3_muladds( pa
, p1
, manifold
->t1
, pa
);
1054 float d
= v3_length( manifold
->d1
);
1055 v3_muls( manifold
->d1
, 1.0f
/d
, ct
->n
);
1056 v3_muladds( pa
, ct
->n
, -ra
, ct
->co
);
1058 ct
->p
= manifold
->r1
- d
;
1071 vg_line( buf
[0].co
, buf
[1].co
, 0xff0000ff );
1076 VG_STATIC
int rb_capsule_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1078 float h
= rba
->inf
.capsule
.height
,
1079 ra
= rba
->inf
.capsule
.radius
,
1080 rb
= rbb
->inf
.sphere
.radius
;
1083 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+ra
, p0
);
1084 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-ra
, p1
);
1087 closest_point_segment( p0
, p1
, rbb
->co
, c
);
1088 v3_sub( c
, rbb
->co
, delta
);
1090 float d2
= v3_length2(delta
),
1095 float d
= sqrtf(d2
);
1098 v3_muls( delta
, 1.0f
/d
, ct
->n
);
1102 v3_muladds( c
, ct
->n
, -ra
, p0
);
1103 v3_muladds( rbb
->co
, ct
->n
, rb
, p1
);
1104 v3_add( p0
, p1
, ct
->co
);
1105 v3_muls( ct
->co
, 0.5f
, ct
->co
);
1117 VG_STATIC
int rb_capsule_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1119 float ha
= rba
->inf
.capsule
.height
,
1120 hb
= rbb
->inf
.capsule
.height
,
1121 ra
= rba
->inf
.capsule
.radius
,
1122 rb
= rbb
->inf
.capsule
.radius
,
1126 v3_muladds( rba
->co
, rba
->up
, -ha
*0.5f
+ra
, p0
);
1127 v3_muladds( rba
->co
, rba
->up
, ha
*0.5f
-ra
, p1
);
1128 v3_muladds( rbb
->co
, rbb
->up
, -hb
*0.5f
+rb
, p2
);
1129 v3_muladds( rbb
->co
, rbb
->up
, hb
*0.5f
-rb
, p3
);
1131 capsule_manifold manifold
;
1132 rb_capsule_manifold_init( &manifold
);
1136 closest_segment_segment( p0
, p1
, p2
, p3
, &ta
, &tb
, pa
, pb
);
1137 rb_capsule_manifold( pa
, pb
, ta
, r
, &manifold
);
1139 ta
= closest_point_segment( p0
, p1
, p2
, pa
);
1140 tb
= closest_point_segment( p0
, p1
, p3
, pb
);
1141 rb_capsule_manifold( pa
, p2
, ta
, r
, &manifold
);
1142 rb_capsule_manifold( pb
, p3
, tb
, r
, &manifold
);
1144 closest_point_segment( p2
, p3
, p0
, pa
);
1145 closest_point_segment( p2
, p3
, p1
, pb
);
1146 rb_capsule_manifold( p0
, pa
, 0.0f
, r
, &manifold
);
1147 rb_capsule_manifold( p1
, pb
, 1.0f
, r
, &manifold
);
1149 return rb_capsule_manifold_done( rba
, rbb
, &manifold
, buf
);
1153 * Generates up to two contacts; optimised for the most stable manifold
1155 VG_STATIC
int rb_capsule_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1157 float h
= rba
->inf
.capsule
.height
,
1158 r
= rba
->inf
.capsule
.radius
;
1161 * Solving this in symetric local space of the cube saves us some time and a
1162 * couple branches when it comes to the quad stage.
1165 v3_add( rbb
->bbx
[0], rbb
->bbx
[1], centroid
);
1166 v3_muls( centroid
, 0.5f
, centroid
);
1169 v3_sub( rbb
->bbx
[0], centroid
, bbx
[0] );
1170 v3_sub( rbb
->bbx
[1], centroid
, bbx
[1] );
1172 v3f pc
, p0w
, p1w
, p0
, p1
;
1173 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+r
, p0w
);
1174 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-r
, p1w
);
1176 m4x3_mulv( rbb
->to_local
, p0w
, p0
);
1177 m4x3_mulv( rbb
->to_local
, p1w
, p1
);
1178 v3_sub( p0
, centroid
, p0
);
1179 v3_sub( p1
, centroid
, p1
);
1180 v3_add( p0
, p1
, pc
);
1181 v3_muls( pc
, 0.5f
, pc
);
1184 * Finding an appropriate quad to collide lines with
1187 v3_div( pc
, bbx
[1], region
);
1190 if( (fabsf(region
[0]) > fabsf(region
[1])) &&
1191 (fabsf(region
[0]) > fabsf(region
[2])) )
1193 float px
= vg_signf(region
[0]) * bbx
[1][0];
1194 v3_copy( (v3f
){ px
, bbx
[0][1], bbx
[0][2] }, quad
[0] );
1195 v3_copy( (v3f
){ px
, bbx
[1][1], bbx
[0][2] }, quad
[1] );
1196 v3_copy( (v3f
){ px
, bbx
[1][1], bbx
[1][2] }, quad
[2] );
1197 v3_copy( (v3f
){ px
, bbx
[0][1], bbx
[1][2] }, quad
[3] );
1199 else if( fabsf(region
[1]) > fabsf(region
[2]) )
1201 float py
= vg_signf(region
[1]) * bbx
[1][1];
1202 v3_copy( (v3f
){ bbx
[0][0], py
, bbx
[0][2] }, quad
[0] );
1203 v3_copy( (v3f
){ bbx
[1][0], py
, bbx
[0][2] }, quad
[1] );
1204 v3_copy( (v3f
){ bbx
[1][0], py
, bbx
[1][2] }, quad
[2] );
1205 v3_copy( (v3f
){ bbx
[0][0], py
, bbx
[1][2] }, quad
[3] );
1209 float pz
= vg_signf(region
[2]) * bbx
[1][2];
1210 v3_copy( (v3f
){ bbx
[0][0], bbx
[0][1], pz
}, quad
[0] );
1211 v3_copy( (v3f
){ bbx
[1][0], bbx
[0][1], pz
}, quad
[1] );
1212 v3_copy( (v3f
){ bbx
[1][0], bbx
[1][1], pz
}, quad
[2] );
1213 v3_copy( (v3f
){ bbx
[0][0], bbx
[1][1], pz
}, quad
[3] );
1216 capsule_manifold manifold
;
1217 rb_capsule_manifold_init( &manifold
);
1220 closest_point_aabb( p0
, bbx
, c0
);
1221 closest_point_aabb( p1
, bbx
, c1
);
1224 v3_sub( c0
, p0
, d0
);
1225 v3_sub( c1
, p1
, d1
);
1226 v3_sub( p1
, p0
, da
);
1232 if( v3_dot( da
, d0
) <= 0.01f
)
1233 rb_capsule_manifold( p0
, c0
, 0.0f
, r
, &manifold
);
1235 if( v3_dot( da
, d1
) >= -0.01f
)
1236 rb_capsule_manifold( p1
, c1
, 1.0f
, r
, &manifold
);
1238 for( int i
=0; i
<4; i
++ )
1245 closest_segment_segment( p0
, p1
, quad
[i0
], quad
[i1
], &ta
, &tb
, ca
, cb
);
1246 rb_capsule_manifold( ca
, cb
, ta
, r
, &manifold
);
1250 * Create final contacts based on line manifold
1252 m3x3_mulv( rbb
->to_world
, manifold
.d0
, manifold
.d0
);
1253 m3x3_mulv( rbb
->to_world
, manifold
.d1
, manifold
.d1
);
1260 for( int i
=0; i
<4; i
++ )
1266 v3_add( quad
[i0
], centroid
, q0
);
1267 v3_add( quad
[i1
], centroid
, q1
);
1269 m4x3_mulv( rbb
->to_world
, q0
, q0
);
1270 m4x3_mulv( rbb
->to_world
, q1
, q1
);
1272 vg_line( q0
, q1
, 0xffffffff );
1276 return rb_capsule_manifold_done( rba
, rbb
, &manifold
, buf
);
1279 VG_STATIC
int rb_sphere_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1283 closest_point_obb( rba
->co
, rbb
, co
);
1284 v3_sub( rba
->co
, co
, delta
);
1286 float d2
= v3_length2(delta
),
1287 r
= rba
->inf
.sphere
.radius
;
1296 v3_sub( rba
->co
, rbb
->co
, delta
);
1299 * some extra testing is required to find the best axis to push the
1300 * object back outside the box. Since there isnt a clear seperating
1301 * vector already, especially on really high aspect boxes.
1303 float lx
= v3_dot( rbb
->right
, delta
),
1304 ly
= v3_dot( rbb
->up
, delta
),
1305 lz
= v3_dot( rbb
->forward
, delta
),
1306 px
= rbb
->bbx
[1][0] - fabsf(lx
),
1307 py
= rbb
->bbx
[1][1] - fabsf(ly
),
1308 pz
= rbb
->bbx
[1][2] - fabsf(lz
);
1310 if( px
< py
&& px
< pz
)
1311 v3_muls( rbb
->right
, vg_signf(lx
), ct
->n
);
1313 v3_muls( rbb
->up
, vg_signf(ly
), ct
->n
);
1315 v3_muls( rbb
->forward
, vg_signf(lz
), ct
->n
);
1317 v3_muladds( rba
->co
, ct
->n
, -r
, ct
->co
);
1323 v3_muls( delta
, 1.0f
/d
, ct
->n
);
1325 v3_copy( co
, ct
->co
);
1337 VG_STATIC
int rb_sphere_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1340 v3_sub( rba
->co
, rbb
->co
, delta
);
1342 float d2
= v3_length2(delta
),
1343 r
= rba
->inf
.sphere
.radius
+ rbb
->inf
.sphere
.radius
;
1347 float d
= sqrtf(d2
);
1350 v3_muls( delta
, 1.0f
/d
, ct
->n
);
1353 v3_muladds( rba
->co
, ct
->n
,-rba
->inf
.sphere
.radius
, p0
);
1354 v3_muladds( rbb
->co
, ct
->n
, rbb
->inf
.sphere
.radius
, p1
);
1355 v3_add( p0
, p1
, ct
->co
);
1356 v3_muls( ct
->co
, 0.5f
, ct
->co
);
1367 #define RIGIDBODY_DYNAMIC_MESH_EDGES
1369 VG_STATIC
int rb_sphere_triangle( rigidbody
*rba
, rigidbody
*rbb
,
1370 v3f tri
[3], rb_ct
*buf
)
1374 #ifdef RIGIDBODY_DYNAMIC_MESH_EDGES
1375 closest_on_triangle( rba
->co
, tri
, co
);
1377 closest_on_triangle_1( rba
->co
, tri
, co
);
1380 v3_sub( rba
->co
, co
, delta
);
1382 vg_line( rba
->co
, co
, 0xffff0000 );
1383 vg_line_pt3( rba
->co
, 0.1f
, 0xff00ffff );
1385 float d2
= v3_length2( delta
),
1386 r
= rba
->inf
.sphere
.radius
;
1393 v3_sub( tri
[2], tri
[0], ab
);
1394 v3_sub( tri
[1], tri
[0], ac
);
1395 v3_cross( ac
, ab
, tn
);
1396 v3_copy( tn
, ct
->n
);
1397 v3_normalize( ct
->n
);
1399 float d
= sqrtf(d2
);
1401 v3_copy( co
, ct
->co
);
1412 VG_STATIC
int rb_sphere_scene( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1414 scene
*sc
= rbb
->inf
.scene
.bh_scene
->user
;
1418 int len
= bh_select( rbb
->inf
.scene
.bh_scene
, rba
->bbx_world
, geo
, 128 );
1421 #ifdef RIGIDBODY_DYNAMIC_MESH_EDGES
1422 /* !experimental! build edge array on the fly. time could be improved! */
1424 v3f co_picture
[128*3];
1429 int unique_cos
[3]; /* indexes co_picture array */
1436 /* create geometry picture */
1437 for( int i
=0; i
<len
; i
++ )
1439 u32
*tri_indices
= &sc
->arrindices
[ geo
[i
]*3 ];
1440 struct face_info
*inf
= &faces
[i
];
1441 inf
->element_id
= tri_indices
[0];
1444 for( int j
=0; j
<3; j
++ )
1446 struct mdl_vert
*pvert
= &sc
->arrvertices
[tri_indices
[j
]];
1448 for( int k
=0; k
<unique_cos
; k
++ )
1450 if( v3_dist( pvert
->co
, co_picture
[k
] ) < 0.01f
*0.01f
)
1452 inf
->unique_cos
[j
] = k
;
1457 inf
->unique_cos
[j
] = unique_cos
;
1458 v3_copy( pvert
->co
, co_picture
[ unique_cos
++ ] );
1463 v3_sub( co_picture
[inf
->unique_cos
[2]],
1464 co_picture
[inf
->unique_cos
[0]], ab
);
1466 v3_sub( co_picture
[inf
->unique_cos
[1]],
1467 co_picture
[inf
->unique_cos
[0]], ac
);
1468 v3_cross( ac
, ab
, inf
->normal
);
1469 v3_normalize( inf
->normal
);
1473 /* build edges brute force */
1474 int edge_picture
[ 128*3 ][4];
1475 int unique_edges
= 0;
1477 for( int i
=0; i
<len
; i
++ )
1479 struct face_info
*inf
= &faces
[i
];
1481 for( int j
=0; j
<3; j
++ )
1485 e0
= VG_MIN( inf
->unique_cos
[i0
], inf
->unique_cos
[i1
] ),
1486 e1
= VG_MAX( inf
->unique_cos
[i0
], inf
->unique_cos
[i1
] ),
1489 for( int k
=0; k
<unique_edges
; k
++ )
1491 int k0
= VG_MIN( edge_picture
[k
][0], edge_picture
[k
][1] ),
1492 k1
= VG_MAX( edge_picture
[k
][0], edge_picture
[k
][1] );
1495 if( (k0
== e0
) && (k1
== e1
) )
1497 edge_picture
[ k
][3] = i
;
1505 /* create new edge */
1506 edge_picture
[ unique_edges
][0] = inf
->unique_cos
[i0
];
1507 edge_picture
[ unique_edges
][1] = inf
->unique_cos
[i1
];
1509 edge_picture
[ unique_edges
][2] = i
;
1510 edge_picture
[ unique_edges
][3] = -1;
1520 for( int i
=0; i
<len
; i
++ )
1522 #ifdef RIGIDBODY_DYNAMIC_MESH_EDGES
1523 struct face_info
*inf
= &faces
[i
];
1525 float *v0
= co_picture
[inf
->unique_cos
[0]],
1526 *v1
= co_picture
[inf
->unique_cos
[1]],
1527 *v2
= co_picture
[inf
->unique_cos
[2]];
1529 v3_copy( v0
, tri
[0] );
1530 v3_copy( v1
, tri
[1] );
1531 v3_copy( v2
, tri
[2] );
1533 buf
[count
].element_id
= inf
->element_id
;
1535 u32
*ptri
= &sc
->arrindices
[ geo
[i
]*3 ];
1537 for( int j
=0; j
<3; j
++ )
1538 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
1540 buf
[count
].element_id
= ptri
[0];
1543 vg_line( tri
[0],tri
[1],0x10ffffff );
1544 vg_line( tri
[1],tri
[2],0x10ffffff );
1545 vg_line( tri
[2],tri
[0],0x10ffffff );
1547 int contact
= rb_sphere_triangle( rba
, rbb
, tri
, buf
+count
);
1549 #ifdef RIGIDBODY_DYNAMIC_MESH_EDGES
1557 vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
1562 #ifdef RIGIDBODY_DYNAMIC_MESH_EDGES
1563 for( int i
=0; i
<unique_edges
; i
++ )
1565 int *edge
= edge_picture
[i
];
1570 struct face_info
*inf_i
= &faces
[edge
[2]],
1571 *inf_j
= &faces
[edge
[3]];
1573 if( inf_i
->collided
|| inf_j
->collided
)
1577 closest_point_segment( co_picture
[edge
[0]], co_picture
[edge
[1]],
1580 v3_sub( rba
->co
, co
, delta
);
1581 float d2
= v3_length2( delta
),
1582 r
= rba
->inf
.sphere
.radius
;
1586 float d
= sqrtf(d2
);
1588 v3_muls( delta
, 1.0f
/d
, delta
);
1589 float c0
= v3_dot( inf_i
->normal
, delta
),
1590 c1
= v3_dot( inf_j
->normal
, delta
);
1592 if( c0
< 0.0f
|| c1
< 0.0f
)
1595 rb_ct
*ct
= buf
+count
;
1597 v3_muls( inf_i
->normal
, c0
, ct
->n
);
1598 v3_muladds( ct
->n
, inf_j
->normal
, c1
, ct
->n
);
1599 v3_normalize( ct
->n
);
1601 v3_copy( co
, ct
->co
);
1606 ct
->element_id
= inf_i
->element_id
;
1612 vg_warn( "Geometry too dense!\n" );
1622 VG_STATIC
int rb_box_scene( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1624 scene
*sc
= rbb
->inf
.scene
.bh_scene
->user
;
1628 int len
= bh_select( rbb
->inf
.scene
.bh_scene
, rba
->bbx_world
, geo
, 128 );
1632 for( int i
=0; i
<len
; i
++ )
1634 u32
*ptri
= &sc
->arrindices
[ geo
[i
]*3 ];
1636 for( int j
=0; j
<3; j
++ )
1637 v3_copy( sc
->arrvertices
[ptri
[j
]].co
, tri
[j
] );
1639 if( rb_box_triangle_sat( rba
, tri
) )
1641 vg_line(tri
[0],tri
[1],0xff50ff00 );
1642 vg_line(tri
[1],tri
[2],0xff50ff00 );
1643 vg_line(tri
[2],tri
[0],0xff50ff00 );
1647 vg_line(tri
[0],tri
[1],0xff0000ff );
1648 vg_line(tri
[1],tri
[2],0xff0000ff );
1649 vg_line(tri
[2],tri
[0],0xff0000ff );
1655 v3_sub( tri
[1], tri
[0], v0
);
1656 v3_sub( tri
[2], tri
[0], v1
);
1657 v3_cross( v0
, v1
, n
);
1660 /* find best feature */
1661 float best
= v3_dot( rba
->right
, n
);
1664 float cy
= v3_dot( rba
->up
, n
);
1665 if( fabsf(cy
) > fabsf(best
) )
1671 float cz
= -v3_dot( rba
->forward
, n
);
1672 if( fabsf(cz
) > fabsf(best
) )
1682 float px
= best
> 0.0f
? rba
->bbx
[0][0]: rba
->bbx
[1][0];
1683 manifold
[0][0] = px
;
1684 manifold
[0][1] = rba
->bbx
[0][1];
1685 manifold
[0][2] = rba
->bbx
[0][2];
1686 manifold
[1][0] = px
;
1687 manifold
[1][1] = rba
->bbx
[1][1];
1688 manifold
[1][2] = rba
->bbx
[0][2];
1689 manifold
[2][0] = px
;
1690 manifold
[2][1] = rba
->bbx
[1][1];
1691 manifold
[2][2] = rba
->bbx
[1][2];
1692 manifold
[3][0] = px
;
1693 manifold
[3][1] = rba
->bbx
[0][1];
1694 manifold
[3][2] = rba
->bbx
[1][2];
1696 else if( axis
== 1 )
1698 float py
= best
> 0.0f
? rba
->bbx
[0][1]: rba
->bbx
[1][1];
1699 manifold
[0][0] = rba
->bbx
[0][0];
1700 manifold
[0][1] = py
;
1701 manifold
[0][2] = rba
->bbx
[0][2];
1702 manifold
[1][0] = rba
->bbx
[1][0];
1703 manifold
[1][1] = py
;
1704 manifold
[1][2] = rba
->bbx
[0][2];
1705 manifold
[2][0] = rba
->bbx
[1][0];
1706 manifold
[2][1] = py
;
1707 manifold
[2][2] = rba
->bbx
[1][2];
1708 manifold
[3][0] = rba
->bbx
[0][0];
1709 manifold
[3][1] = py
;
1710 manifold
[3][2] = rba
->bbx
[1][2];
1714 float pz
= best
> 0.0f
? rba
->bbx
[0][2]: rba
->bbx
[1][2];
1715 manifold
[0][0] = rba
->bbx
[0][0];
1716 manifold
[0][1] = rba
->bbx
[0][1];
1717 manifold
[0][2] = pz
;
1718 manifold
[1][0] = rba
->bbx
[1][0];
1719 manifold
[1][1] = rba
->bbx
[0][1];
1720 manifold
[1][2] = pz
;
1721 manifold
[2][0] = rba
->bbx
[1][0];
1722 manifold
[2][1] = rba
->bbx
[1][1];
1723 manifold
[2][2] = pz
;
1724 manifold
[3][0] = rba
->bbx
[0][0];
1725 manifold
[3][1] = rba
->bbx
[1][1];
1726 manifold
[3][2] = pz
;
1729 for( int j
=0; j
<4; j
++ )
1730 m4x3_mulv( rba
->to_world
, manifold
[j
], manifold
[j
] );
1732 vg_line( manifold
[0], manifold
[1], 0xffffffff );
1733 vg_line( manifold
[1], manifold
[2], 0xffffffff );
1734 vg_line( manifold
[2], manifold
[3], 0xffffffff );
1735 vg_line( manifold
[3], manifold
[0], 0xffffffff );
1737 for( int j
=0; j
<4; j
++ )
1739 rb_ct
*ct
= buf
+count
;
1741 v3_copy( manifold
[j
], ct
->co
);
1742 v3_copy( n
, ct
->n
);
1744 float l0
= v3_dot( tri
[0], n
),
1745 l1
= v3_dot( manifold
[j
], n
);
1747 ct
->p
= (l0
-l1
)*0.5f
;
1763 VG_STATIC
int RB_MATRIX_ERROR( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1765 vg_error( "Collision type is unimplemented between types %d and %d\n",
1766 rba
->type
, rbb
->type
);
1771 VG_STATIC
int rb_sphere_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1773 return rb_capsule_sphere( rbb
, rba
, buf
);
1776 VG_STATIC
int rb_box_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1778 return rb_capsule_box( rbb
, rba
, buf
);
1781 VG_STATIC
int rb_box_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1783 return rb_sphere_box( rbb
, rba
, buf
);
1786 VG_STATIC
int rb_scene_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1788 return rb_box_scene( rbb
, rba
, buf
);
1791 VG_STATIC
int (*rb_jump_table
[4][4])( rigidbody
*a
, rigidbody
*b
, rb_ct
*buf
) =
1793 /* box */ /* Sphere */ /* Capsule */ /* Mesh */
1794 { RB_MATRIX_ERROR
, rb_box_sphere
, rb_box_capsule
, rb_box_scene
},
1795 { rb_sphere_box
, rb_sphere_sphere
, rb_sphere_capsule
, rb_sphere_scene
},
1796 { rb_capsule_box
, rb_capsule_sphere
, rb_capsule_capsule
, RB_MATRIX_ERROR
},
1797 { rb_scene_box
, RB_MATRIX_ERROR
, RB_MATRIX_ERROR
, RB_MATRIX_ERROR
}
1800 VG_STATIC
int rb_collide( rigidbody
*rba
, rigidbody
*rbb
)
1802 int (*collider_jump
)(rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1803 = rb_jump_table
[rba
->type
][rbb
->type
];
1806 * 12 is the maximum manifold size we can generate, so we are forced to abort
1807 * potentially checking any more.
1809 if( rb_contact_count
+ 12 > vg_list_size(rb_contact_buffer
) )
1811 vg_warn( "Too many contacts made in global collider buffer (%d of %d\n)",
1812 rb_contact_count
, vg_list_size(rb_contact_buffer
) );
1817 * FUTURE: Replace this with a more dedicated broad phase pass
1819 if( box_overlap( rba
->bbx_world
, rbb
->bbx_world
) )
1821 int count
= collider_jump( rba
, rbb
, rb_contact_buffer
+rb_contact_count
);
1822 rb_contact_count
+= count
;
1830 * -----------------------------------------------------------------------------
1832 * -----------------------------------------------------------------------------
1835 VG_STATIC
void rb_solver_reset(void)
1837 rb_contact_count
= 0;
1840 VG_STATIC rb_ct
*rb_global_ct(void)
1842 return rb_contact_buffer
+ rb_contact_count
;
1846 * Initializing things like tangent vectors
1848 VG_STATIC
void rb_presolve_contacts( rb_ct
*buffer
, int len
)
1850 for( int i
=0; i
<len
; i
++ )
1852 rb_ct
*ct
= &buffer
[i
];
1854 ct
->bias
= -0.2f
* k_rb_rate
* vg_minf( 0.0f
, -ct
->p
+k_penetration_slop
);
1855 rb_tangent_basis( ct
->n
, ct
->t
[0], ct
->t
[1] );
1858 ct
->norm_impulse
= 0.0f
;
1859 ct
->tangent_impulse
[0] = 0.0f
;
1860 ct
->tangent_impulse
[1] = 0.0f
;
1862 v3f ra
, rb
, raCn
, rbCn
, raCt
, rbCt
;
1863 v3_sub( ct
->co
, ct
->rba
->co
, ra
);
1864 v3_sub( ct
->co
, ct
->rbb
->co
, rb
);
1865 v3_cross( ra
, ct
->n
, raCn
);
1866 v3_cross( rb
, ct
->n
, rbCn
);
1868 /* orient inverse inertia tensors */
1870 m3x3_mulv( ct
->rba
->iIw
, raCn
, raCnI
);
1871 m3x3_mulv( ct
->rbb
->iIw
, rbCn
, rbCnI
);
1873 ct
->normal_mass
= ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
1874 ct
->normal_mass
+= v3_dot( raCn
, raCnI
);
1875 ct
->normal_mass
+= v3_dot( rbCn
, rbCnI
);
1876 ct
->normal_mass
= 1.0f
/ct
->normal_mass
;
1878 for( int j
=0; j
<2; j
++ )
1881 v3_cross( ct
->t
[j
], ra
, raCt
);
1882 v3_cross( ct
->t
[j
], rb
, rbCt
);
1883 m3x3_mulv( ct
->rba
->iIw
, raCt
, raCtI
);
1884 m3x3_mulv( ct
->rbb
->iIw
, rbCt
, rbCtI
);
1886 ct
->tangent_mass
[j
] = ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
;
1887 ct
->tangent_mass
[j
] += v3_dot( raCt
, raCtI
);
1888 ct
->tangent_mass
[j
] += v3_dot( rbCt
, rbCtI
);
1889 ct
->tangent_mass
[j
] = 1.0f
/ct
->tangent_mass
[j
];
1892 rb_debug_contact( ct
);
1897 * Creates relative contact velocity vector, and offsets between each body
1899 VG_STATIC
void rb_rcv( rb_ct
*ct
, v3f rv
, v3f da
, v3f db
)
1901 rigidbody
*rba
= ct
->rba
,
1904 v3_sub( ct
->co
, rba
->co
, da
);
1905 v3_sub( ct
->co
, rbb
->co
, db
);
1908 v3_cross( rba
->w
, da
, rva
);
1909 v3_add( rba
->v
, rva
, rva
);
1910 v3_cross( rbb
->w
, db
, rvb
);
1911 v3_add( rbb
->v
, rvb
, rvb
);
1913 v3_sub( rva
, rvb
, rv
);
1917 * Apply impulse to object
1919 VG_STATIC
void rb_linear_impulse( rigidbody
*rb
, v3f delta
, v3f impulse
)
1922 v3_muladds( rb
->v
, impulse
, rb
->inv_mass
, rb
->v
);
1924 /* Angular velocity */
1926 v3_cross( delta
, impulse
, wa
);
1928 m3x3_mulv( rb
->iIw
, wa
, wa
);
1929 v3_add( rb
->w
, wa
, rb
->w
);
1933 * One iteration to solve the contact constraint
1935 VG_STATIC
void rb_solve_contacts( rb_ct
*buf
, int len
)
1937 for( int i
=0; i
<len
; i
++ )
1939 struct contact
*ct
= &buf
[i
];
1941 rigidbody
*rb
= ct
->rba
;
1944 rb_rcv( ct
, rv
, da
, db
);
1947 for( int j
=0; j
<2; j
++ )
1949 float f
= k_friction
* ct
->norm_impulse
,
1950 vt
= v3_dot( rv
, ct
->t
[j
] ),
1951 lambda
= ct
->tangent_mass
[j
] * -vt
;
1953 float temp
= ct
->tangent_impulse
[j
];
1954 ct
->tangent_impulse
[j
] = vg_clampf( temp
+ lambda
, -f
, f
);
1955 lambda
= ct
->tangent_impulse
[j
] - temp
;
1958 v3_muls( ct
->t
[j
], lambda
, impulse
);
1959 rb_linear_impulse( ct
->rba
, da
, impulse
);
1961 v3_muls( ct
->t
[j
], -lambda
, impulse
);
1962 rb_linear_impulse( ct
->rbb
, db
, impulse
);
1966 rb_rcv( ct
, rv
, da
, db
);
1967 float vn
= v3_dot( rv
, ct
->n
),
1968 lambda
= ct
->normal_mass
* (-vn
+ ct
->bias
);
1970 float temp
= ct
->norm_impulse
;
1971 ct
->norm_impulse
= vg_maxf( temp
+ lambda
, 0.0f
);
1972 lambda
= ct
->norm_impulse
- temp
;
1975 v3_muls( ct
->n
, lambda
, impulse
);
1976 rb_linear_impulse( ct
->rba
, da
, impulse
);
1978 v3_muls( ct
->n
, -lambda
, impulse
);
1979 rb_linear_impulse( ct
->rbb
, db
, impulse
);
1984 * -----------------------------------------------------------------------------
1986 * -----------------------------------------------------------------------------
1989 VG_STATIC
void draw_angle_limit( v3f c
, v3f major
, v3f minor
,
1990 float amin
, float amax
, float measured
,
1995 v3_muls( major
, f
, ay
);
1996 v3_muls( minor
, f
, ax
);
1998 for( int x
=0; x
<16; x
++ )
2000 float t0
= (float)x
/ 16.0f
,
2001 t1
= (float)(x
+1) / 16.0f
,
2002 a0
= vg_lerpf( amin
, amax
, t0
),
2003 a1
= vg_lerpf( amin
, amax
, t1
);
2006 v3_muladds( c
, ay
, cosf(a0
), p0
);
2007 v3_muladds( p0
, ax
, sinf(a0
), p0
);
2008 v3_muladds( c
, ay
, cosf(a1
), p1
);
2009 v3_muladds( p1
, ax
, sinf(a1
), p1
);
2011 vg_line( p0
, p1
, colour
);
2014 vg_line( c
, p0
, colour
);
2016 vg_line( c
, p1
, colour
);
2020 v3_muladds( c
, ay
, cosf(measured
)*1.2f
, p2
);
2021 v3_muladds( p2
, ax
, sinf(measured
)*1.2f
, p2
);
2022 vg_line( c
, p2
, colour
);
2025 VG_STATIC
void rb_debug_constraint_limits( rigidbody
*ra
, rigidbody
*rb
, v3f lca
,
2028 v3f ax
, ay
, az
, bx
, by
, bz
;
2029 m3x3_mulv( ra
->to_world
, (v3f
){1.0f
,0.0f
,0.0f
}, ax
);
2030 m3x3_mulv( ra
->to_world
, (v3f
){0.0f
,1.0f
,0.0f
}, ay
);
2031 m3x3_mulv( ra
->to_world
, (v3f
){0.0f
,0.0f
,1.0f
}, az
);
2032 m3x3_mulv( rb
->to_world
, (v3f
){1.0f
,0.0f
,0.0f
}, bx
);
2033 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,1.0f
,0.0f
}, by
);
2034 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,0.0f
,1.0f
}, bz
);
2037 px
[0] = v3_dot( ay
, by
);
2038 px
[1] = v3_dot( az
, by
);
2040 py
[0] = v3_dot( az
, bz
);
2041 py
[1] = v3_dot( ax
, bz
);
2043 pz
[0] = v3_dot( ax
, bx
);
2044 pz
[1] = v3_dot( ay
, bx
);
2046 float r0
= atan2f( px
[1], px
[0] ),
2047 r1
= atan2f( py
[1], py
[0] ),
2048 r2
= atan2f( pz
[1], pz
[0] );
2051 m4x3_mulv( ra
->to_world
, lca
, c
);
2052 draw_angle_limit( c
, ay
, az
, limits
[0][0], limits
[1][0], r0
, 0xff0000ff );
2053 draw_angle_limit( c
, az
, ax
, limits
[0][1], limits
[1][1], r1
, 0xff00ff00 );
2054 draw_angle_limit( c
, ax
, ay
, limits
[0][2], limits
[1][2], r2
, 0xffff0000 );
2057 VG_STATIC
void rb_limit_cure( rigidbody
*ra
, rigidbody
*rb
, v3f axis
, float d
)
2061 float avx
= v3_dot( ra
->w
, axis
) - v3_dot( rb
->w
, axis
);
2062 float joint_mass
= rb
->inv_mass
+ ra
->inv_mass
;
2063 joint_mass
= 1.0f
/joint_mass
;
2065 float bias
= (k_limit_bias
* k_rb_rate
) * d
,
2066 lambda
= -(avx
+ bias
) * joint_mass
;
2068 /* Angular velocity */
2070 v3_muls( axis
, lambda
* ra
->inv_mass
, wa
);
2071 v3_muls( axis
, -lambda
* rb
->inv_mass
, wb
);
2073 v3_add( ra
->w
, wa
, ra
->w
);
2074 v3_add( rb
->w
, wb
, rb
->w
);
2078 VG_STATIC
void rb_constraint_limits( rigidbody
*ra
, v3f lca
,
2079 rigidbody
*rb
, v3f lcb
, v3f limits
[2] )
2081 v3f ax
, ay
, az
, bx
, by
, bz
;
2082 m3x3_mulv( ra
->to_world
, (v3f
){1.0f
,0.0f
,0.0f
}, ax
);
2083 m3x3_mulv( ra
->to_world
, (v3f
){0.0f
,1.0f
,0.0f
}, ay
);
2084 m3x3_mulv( ra
->to_world
, (v3f
){0.0f
,0.0f
,1.0f
}, az
);
2085 m3x3_mulv( rb
->to_world
, (v3f
){1.0f
,0.0f
,0.0f
}, bx
);
2086 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,1.0f
,0.0f
}, by
);
2087 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,0.0f
,1.0f
}, bz
);
2090 px
[0] = v3_dot( ay
, by
);
2091 px
[1] = v3_dot( az
, by
);
2093 py
[0] = v3_dot( az
, bz
);
2094 py
[1] = v3_dot( ax
, bz
);
2096 pz
[0] = v3_dot( ax
, bx
);
2097 pz
[1] = v3_dot( ay
, bx
);
2099 float r0
= atan2f( px
[1], px
[0] ),
2100 r1
= atan2f( py
[1], py
[0] ),
2101 r2
= atan2f( pz
[1], pz
[0] );
2103 /* calculate angle deltas */
2104 float dx
= 0.0f
, dy
= 0.0f
, dz
= 0.0f
;
2106 if( r0
< limits
[0][0] ) dx
= limits
[0][0] - r0
;
2107 if( r0
> limits
[1][0] ) dx
= limits
[1][0] - r0
;
2108 if( r1
< limits
[0][1] ) dy
= limits
[0][1] - r1
;
2109 if( r1
> limits
[1][1] ) dy
= limits
[1][1] - r1
;
2110 if( r2
< limits
[0][2] ) dz
= limits
[0][2] - r2
;
2111 if( r2
> limits
[1][2] ) dz
= limits
[1][2] - r2
;
2114 m3x3_mulv( ra
->to_world
, lca
, wca
);
2115 m3x3_mulv( rb
->to_world
, lcb
, wcb
);
2117 rb_limit_cure( ra
, rb
, ax
, dx
);
2118 rb_limit_cure( ra
, rb
, ay
, dy
);
2119 rb_limit_cure( ra
, rb
, az
, dz
);
2122 VG_STATIC
void rb_debug_constraint_position( rigidbody
*ra
, v3f lca
,
2123 rigidbody
*rb
, v3f lcb
)
2126 m3x3_mulv( ra
->to_world
, lca
, wca
);
2127 m3x3_mulv( rb
->to_world
, lcb
, wcb
);
2130 v3_add( wca
, ra
->co
, p0
);
2131 v3_add( wcb
, rb
->co
, p1
);
2132 vg_line_pt3( p0
, 0.005f
, 0xffffff00 );
2133 vg_line_pt3( p1
, 0.005f
, 0xffffff00 );
2134 vg_line( p0
, p1
, 0xffffff00 );
2137 VG_STATIC
void rb_constraint_position( rigidbody
*ra
, v3f lca
,
2138 rigidbody
*rb
, v3f lcb
)
2140 /* C = (COa + Ra*LCa) - (COb + Rb*LCb) = 0 */
2142 m3x3_mulv( ra
->to_world
, lca
, wca
);
2143 m3x3_mulv( rb
->to_world
, lcb
, wcb
);
2146 v3_sub( ra
->v
, rb
->v
, rcv
);
2149 v3_cross( ra
->w
, wca
, rcv_Ra
);
2150 v3_cross( rb
->w
, wcb
, rcv_Rb
);
2151 v3_add( rcv_Ra
, rcv
, rcv
);
2152 v3_sub( rcv
, rcv_Rb
, rcv
);
2156 v3_add( wca
, ra
->co
, p0
);
2157 v3_add( wcb
, rb
->co
, p1
);
2158 v3_sub( p1
, p0
, delta
);
2160 float dist2
= v3_length2( delta
);
2162 if( dist2
> 0.00001f
)
2164 float dist
= sqrtf(dist2
);
2165 v3_muls( delta
, 1.0f
/dist
, delta
);
2167 float joint_mass
= rb
->inv_mass
+ ra
->inv_mass
;
2169 v3f raCn
, rbCn
, raCt
, rbCt
;
2170 v3_cross( wca
, delta
, raCn
);
2171 v3_cross( wcb
, delta
, rbCn
);
2173 /* orient inverse inertia tensors */
2175 m3x3_mulv( ra
->iIw
, raCn
, raCnI
);
2176 m3x3_mulv( rb
->iIw
, rbCn
, rbCnI
);
2177 joint_mass
+= v3_dot( raCn
, raCnI
);
2178 joint_mass
+= v3_dot( rbCn
, rbCnI
);
2179 joint_mass
= 1.0f
/joint_mass
;
2181 float vd
= v3_dot( rcv
, delta
),
2182 bias
= -(k_joint_bias
* k_rb_rate
) * dist
,
2183 lambda
= -(vd
+ bias
) * joint_mass
;
2186 v3_muls( delta
, lambda
, impulse
);
2187 rb_linear_impulse( ra
, wca
, impulse
);
2188 v3_muls( delta
, -lambda
, impulse
);
2189 rb_linear_impulse( rb
, wcb
, impulse
);
2192 v3_muladds( ra
->co
, delta
, dist
* k_joint_correction
, ra
->co
);
2193 v3_muladds( rb
->co
, delta
, -dist
* k_joint_correction
, rb
->co
);
2201 VG_STATIC
void rb_effect_simple_bouyency( rigidbody
*ra
, v4f plane
,
2202 float amt
, float drag
)
2205 float depth
= v3_dot( plane
, ra
->co
) - plane
[3],
2206 lambda
= vg_clampf( -depth
, 0.0f
, 1.0f
) * amt
;
2208 v3_muladds( ra
->v
, plane
, lambda
* k_rb_delta
, ra
->v
);
2211 v3_muls( ra
->v
, 1.0f
-(drag
*k_rb_delta
), ra
->v
);
2215 * -----------------------------------------------------------------------------
2216 * BVH implementation, this is ONLY for VG_STATIC rigidbodies, its to slow for
2218 * -----------------------------------------------------------------------------
2221 VG_STATIC
void rb_bh_expand_bound( void *user
, boxf bound
, u32 item_index
)
2223 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
2224 box_concat( bound
, rb
->bbx_world
);
2227 VG_STATIC
float rb_bh_centroid( void *user
, u32 item_index
, int axis
)
2229 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
2230 return (rb
->bbx_world
[axis
][0] + rb
->bbx_world
[1][axis
]) * 0.5f
;
2233 VG_STATIC
void rb_bh_swap( void *user
, u32 ia
, u32 ib
)
2235 rigidbody temp
, *rba
, *rbb
;
2236 rba
= &((rigidbody
*)user
)[ ia
];
2237 rbb
= &((rigidbody
*)user
)[ ib
];
2244 VG_STATIC
void rb_bh_debug( void *user
, u32 item_index
)
2246 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
2247 rb_debug( rb
, 0xff00ffff );
2250 VG_STATIC bh_system bh_system_rigidbodies
=
2252 .expand_bound
= rb_bh_expand_bound
,
2253 .item_centroid
= rb_bh_centroid
,
2254 .item_swap
= rb_bh_swap
,
2255 .item_debug
= rb_bh_debug
,
2259 #endif /* RIGIDBODY_H */