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 v3f delta
; /* where is the origin of this in relation to a parent body
67 TODO: Move this somewhere other than rigidbody struct
68 it is only used by character.h's ragdoll
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 mass_total
, p
, bias
, norm_impulse
, tangent_impulse
[2];
98 rb_contact_buffer
[256];
99 static int rb_contact_count
= 0;
101 static void rb_update_bounds( rigidbody
*rb
)
103 box_copy( rb
->bbx
, rb
->bbx_world
);
104 m4x3_transform_aabb( rb
->to_world
, rb
->bbx_world
);
107 static void rb_update_transform( rigidbody
*rb
)
109 q_normalize( rb
->q
);
110 q_m3x3( rb
->q
, rb
->to_world
);
111 v3_copy( rb
->co
, rb
->to_world
[3] );
113 m4x3_invert_affine( rb
->to_world
, rb
->to_local
);
115 m3x3_mulv( rb
->to_world
, (v3f
){1.0f
,0.0f
, 0.0f
}, rb
->right
);
116 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,1.0f
, 0.0f
}, rb
->up
);
117 m3x3_mulv( rb
->to_world
, (v3f
){0.0f
,0.0f
,-1.0f
}, rb
->forward
);
119 rb_update_bounds( rb
);
122 static float sphere_volume( float radius
)
124 float r3
= radius
*radius
*radius
;
125 return (4.0f
/3.0f
) * VG_PIf
* r3
;
128 static void rb_init( rigidbody
*rb
)
132 if( rb
->type
== k_rb_shape_box
)
135 v3_sub( rb
->bbx
[1], rb
->bbx
[0], dims
);
136 volume
= dims
[0]*dims
[1]*dims
[2];
138 else if( rb
->type
== k_rb_shape_sphere
)
140 volume
= sphere_volume( rb
->inf
.sphere
.radius
);
141 v3_fill( rb
->bbx
[0], -rb
->inf
.sphere
.radius
);
142 v3_fill( rb
->bbx
[1], rb
->inf
.sphere
.radius
);
144 else if( rb
->type
== k_rb_shape_capsule
)
146 float r
= rb
->inf
.capsule
.radius
,
147 h
= rb
->inf
.capsule
.height
;
148 volume
= sphere_volume( r
) + VG_PIf
* r
*r
* (h
- r
*2.0f
);
150 v3_fill( rb
->bbx
[0], -rb
->inf
.sphere
.radius
);
151 v3_fill( rb
->bbx
[1], rb
->inf
.sphere
.radius
);
155 else if( rb
->type
== k_rb_shape_scene
)
158 box_copy( rb
->inf
.scene
.pscene
->bbx
, rb
->bbx
);
167 rb
->inv_mass
= 1.0f
/(8.0f
*volume
); /* TODO: Things get weird when mass
168 passes a certain point??? */
174 rb_update_transform( rb
);
177 static void rb_iter( rigidbody
*rb
)
179 v3f gravity
= { 0.0f
, -9.6f
, 0.0f
};
180 v3_muladds( rb
->v
, gravity
, k_rb_delta
, rb
->v
);
182 /* intergrate velocity */
183 v3_muladds( rb
->co
, rb
->v
, k_rb_delta
, rb
->co
);
184 v3_lerp( rb
->w
, (v3f
){0.0f
,0.0f
,0.0f
}, 0.0025f
, rb
->w
);
186 /* inegrate inertia */
187 if( v3_length2( rb
->w
) > 0.0f
)
191 v3_copy( rb
->w
, axis
);
193 float mag
= v3_length( axis
);
194 v3_divs( axis
, mag
, axis
);
195 q_axis_angle( rotation
, axis
, mag
*k_rb_delta
);
196 q_mul( rotation
, rb
->q
, rb
->q
);
200 static void rb_torque( rigidbody
*rb
, v3f axis
, float mag
)
202 v3_muladds( rb
->w
, axis
, mag
*k_rb_delta
, rb
->w
);
205 static void rb_tangent_basis( v3f n
, v3f tx
, v3f ty
)
207 /* Compute tangent basis (box2d) */
208 if( fabsf( n
[0] ) >= 0.57735027f
)
222 v3_cross( n
, tx
, ty
);
225 static void rb_solver_reset(void);
227 static void rb_build_manifold_terrain( rigidbody
*rb
);
228 static void rb_build_manifold_terrain_sphere( rigidbody
*rb
);
230 static void rb_solve_contacts( rb_ct
*buf
, int len
);
231 static void rb_presolve_contacts( rb_ct
*buffer
, int len
);
234 * These closest point tests were learned from Real-Time Collision Detection by
237 static float closest_segment_segment( v3f p1
, v3f q1
, v3f p2
, v3f q2
,
238 float *s
, float *t
, v3f c1
, v3f c2
)
241 v3_sub( q1
, p1
, d1
);
242 v3_sub( q2
, p2
, d2
);
245 float a
= v3_length2( d1
),
246 e
= v3_length2( d2
),
249 const float kEpsilon
= 0.0001f
;
251 if( a
<= kEpsilon
&& e
<= kEpsilon
)
259 v3_sub( c1
, c2
, v0
);
261 return v3_length2( v0
);
267 *t
= vg_clampf( f
/ e
, 0.0f
, 1.0f
);
271 float c
= v3_dot( d1
, r
);
275 *s
= vg_clampf( -c
/ a
, 0.0f
, 1.0f
);
279 float b
= v3_dot(d1
,d2
),
284 *s
= vg_clampf((b
*f
- c
*e
)/d
, 0.0f
, 1.0f
);
296 *s
= vg_clampf( -c
/ a
, 0.0f
, 1.0f
);
301 *s
= vg_clampf((b
-c
)/a
,0.0f
,1.0f
);
306 v3_muladds( p1
, d1
, *s
, c1
);
307 v3_muladds( p2
, d2
, *t
, c2
);
310 v3_sub( c1
, c2
, v0
);
311 return v3_length2( v0
);
314 static void closest_point_aabb( v3f p
, boxf box
, v3f dest
)
316 v3_maxv( p
, box
[0], dest
);
317 v3_minv( dest
, box
[1], dest
);
320 static void closest_point_obb( v3f p
, rigidbody
*rb
, v3f dest
)
323 m4x3_mulv( rb
->to_local
, p
, local
);
324 closest_point_aabb( local
, rb
->bbx
, local
);
325 m4x3_mulv( rb
->to_world
, local
, dest
);
328 static float closest_point_segment( v3f a
, v3f b
, v3f point
, v3f dest
)
332 v3_sub( point
, a
, v1
);
334 float t
= v3_dot( v1
, v0
) / v3_length2(v0
);
335 t
= vg_clampf(t
,0.0f
,1.0f
);
336 v3_muladds( a
, v0
, t
, dest
);
340 static void closest_on_triangle( v3f p
, v3f tri
[3], v3f dest
)
345 /* Region outside A */
346 v3_sub( tri
[1], tri
[0], ab
);
347 v3_sub( tri
[2], tri
[0], ac
);
348 v3_sub( p
, tri
[0], ap
);
352 if( d1
<= 0.0f
&& d2
<= 0.0f
)
354 v3_copy( tri
[0], dest
);
355 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
359 /* Region outside B */
363 v3_sub( p
, tri
[1], bp
);
364 d3
= v3_dot( ab
, bp
);
365 d4
= v3_dot( ac
, bp
);
367 if( d3
>= 0.0f
&& d4
<= d3
)
369 v3_copy( tri
[1], dest
);
370 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
374 /* Edge region of AB */
375 float vc
= d1
*d4
- d3
*d2
;
376 if( vc
<= 0.0f
&& d1
>= 0.0f
&& d3
<= 0.0f
)
378 float v
= d1
/ (d1
-d3
);
379 v3_muladds( tri
[0], ab
, v
, dest
);
380 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
384 /* Region outside C */
387 v3_sub( p
, tri
[2], cp
);
391 if( d6
>= 0.0f
&& d5
<= d6
)
393 v3_copy( tri
[2], dest
);
394 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
399 float vb
= d5
*d2
- d1
*d6
;
400 if( vb
<= 0.0f
&& d2
>= 0.0f
&& d6
<= 0.0f
)
402 float w
= d2
/ (d2
-d6
);
403 v3_muladds( tri
[0], ac
, w
, dest
);
404 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
409 float va
= d3
*d6
- d5
*d4
;
410 if( va
<= 0.0f
&& (d4
-d3
) >= 0.0f
&& (d5
-d6
) >= 0.0f
)
412 float w
= (d4
-d3
) / ((d4
-d3
) + (d5
-d6
));
414 v3_sub( tri
[2], tri
[1], bc
);
415 v3_muladds( tri
[1], bc
, w
, dest
);
416 v3_copy( (v3f
){INFINITY
,INFINITY
,INFINITY
}, dest
);
420 /* P inside region, Q via barycentric coordinates uvw */
421 float d
= 1.0f
/(va
+vb
+vc
),
425 v3_muladds( tri
[0], ab
, v
, dest
);
426 v3_muladds( dest
, ac
, w
, dest
);
430 static void closest_on_triangle_1( v3f p
, v3f tri
[3], v3f dest
)
435 /* Region outside A */
436 v3_sub( tri
[1], tri
[0], ab
);
437 v3_sub( tri
[2], tri
[0], ac
);
438 v3_sub( p
, tri
[0], ap
);
442 if( d1
<= 0.0f
&& d2
<= 0.0f
)
444 v3_copy( tri
[0], dest
);
448 /* Region outside B */
452 v3_sub( p
, tri
[1], bp
);
453 d3
= v3_dot( ab
, bp
);
454 d4
= v3_dot( ac
, bp
);
456 if( d3
>= 0.0f
&& d4
<= d3
)
458 v3_copy( tri
[1], dest
);
462 /* Edge region of AB */
463 float vc
= d1
*d4
- d3
*d2
;
464 if( vc
<= 0.0f
&& d1
>= 0.0f
&& d3
<= 0.0f
)
466 float v
= d1
/ (d1
-d3
);
467 v3_muladds( tri
[0], ab
, v
, dest
);
471 /* Region outside C */
474 v3_sub( p
, tri
[2], cp
);
478 if( d6
>= 0.0f
&& d5
<= d6
)
480 v3_copy( tri
[2], dest
);
485 float vb
= d5
*d2
- d1
*d6
;
486 if( vb
<= 0.0f
&& d2
>= 0.0f
&& d6
<= 0.0f
)
488 float w
= d2
/ (d2
-d6
);
489 v3_muladds( tri
[0], ac
, w
, dest
);
494 float va
= d3
*d6
- d5
*d4
;
495 if( va
<= 0.0f
&& (d4
-d3
) >= 0.0f
&& (d5
-d6
) >= 0.0f
)
497 float w
= (d4
-d3
) / ((d4
-d3
) + (d5
-d6
));
499 v3_sub( tri
[2], tri
[1], bc
);
500 v3_muladds( tri
[1], bc
, w
, dest
);
504 /* P inside region, Q via barycentric coordinates uvw */
505 float d
= 1.0f
/(va
+vb
+vc
),
509 v3_muladds( tri
[0], ab
, v
, dest
);
510 v3_muladds( dest
, ac
, w
, dest
);
513 static int rb_intersect_planes( v4f p0
, v4f p1
, v4f p2
, v3f p
)
516 v3_cross( p1
, p2
, u
);
517 float d
= v3_dot( p0
, u
);
519 if( fabsf(d
) < 0.0001f
)
522 v3_muls( u
, p0
[3], p
);
525 v3_muls( p1
, p2
[3], v0
);
526 v3_muladds( v0
, p2
, -p1
[3], v0
);
527 v3_cross( p0
, v0
, v1
);
529 v3_muls( p
, 1.0f
/d
, p
);
534 int rb_intersect_planes_1( v4f a
, v4f b
, v4f c
, v3f p
)
536 float const epsilon
= 0.001;
544 if( d
< epsilon
&& d
> -epsilon
) return 0;
550 v3_muls( bc
, -a
[3], p
);
551 v3_muladds( p
, ca
, -b
[3], p
);
552 v3_muladds( p
, ab
, -c
[3], p
);
562 * These do not automatically allocate contacts, an appropriately sized
563 * buffer must be supplied. The function returns the size of the manifold
564 * which was generated.
566 * The values set on the contacts are: n, co, p, rba, rbb
569 static void rb_debug_contact( rb_ct
*ct
)
572 v3_muladds( ct
->co
, ct
->n
, 0.2f
, p1
);
573 vg_line_pt3( ct
->co
, 0.1f
, 0xff0000ff );
574 vg_line( ct
->co
, p1
, 0xffffffff );
578 * By collecting the minimum(time) and maximum(time) pairs of points, we
579 * build a reduced and stable exact manifold.
582 * rx: minimum distance of these points
583 * dx: the delta between the two points
585 * pairs will only ammend these if they are creating a collision
587 typedef struct capsule_manifold capsule_manifold
;
588 struct capsule_manifold
596 * Expand a line manifold with a new pair. t value is the time along segment
597 * on the oriented object which created this pair.
599 static void rb_capsule_manifold( v3f pa
, v3f pb
, float t
, float r
,
600 capsule_manifold
*manifold
)
603 v3_sub( pa
, pb
, delta
);
605 if( v3_length2(delta
) < r
*r
)
607 if( t
< manifold
->t0
)
609 v3_copy( delta
, manifold
->d0
);
614 if( t
> manifold
->t1
)
616 v3_copy( delta
, manifold
->d1
);
623 static void rb_capsule_manifold_init( capsule_manifold
*manifold
)
625 manifold
->t0
= INFINITY
;
626 manifold
->t1
= -INFINITY
;
629 static int rb_capsule_manifold_done( rigidbody
*rba
, rigidbody
*rbb
,
630 capsule_manifold
*manifold
, rb_ct
*buf
)
632 float h
= rba
->inf
.capsule
.height
,
633 ra
= rba
->inf
.capsule
.radius
;
636 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+ra
, p0
);
637 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-ra
, p1
);
640 if( manifold
->t0
<= 1.0f
)
645 v3_muls( p0
, 1.0f
-manifold
->t0
, pa
);
646 v3_muladds( pa
, p1
, manifold
->t0
, pa
);
648 float d
= v3_length( manifold
->d0
);
649 v3_muls( manifold
->d0
, 1.0f
/d
, ct
->n
);
650 v3_muladds( pa
, ct
->n
, -ra
, ct
->co
);
652 ct
->p
= manifold
->r0
- d
;
659 if( (manifold
->t1
>= 0.0f
) && (manifold
->t0
!= manifold
->t1
) )
661 rb_ct
*ct
= buf
+count
;
664 v3_muls( p0
, 1.0f
-manifold
->t1
, pa
);
665 v3_muladds( pa
, p1
, manifold
->t1
, pa
);
667 float d
= v3_length( manifold
->d1
);
668 v3_muls( manifold
->d1
, 1.0f
/d
, ct
->n
);
669 v3_muladds( pa
, ct
->n
, -ra
, ct
->co
);
671 ct
->p
= manifold
->r1
- d
;
683 vg_line( buf
[0].co
, buf
[1].co
, 0xff0000ff );
688 static int rb_capsule_vs_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
690 float h
= rba
->inf
.capsule
.height
,
691 ra
= rba
->inf
.capsule
.radius
,
692 rb
= rbb
->inf
.sphere
.radius
;
695 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+ra
, p0
);
696 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-ra
, p1
);
699 closest_point_segment( p0
, p1
, rbb
->co
, c
);
700 v3_sub( c
, rbb
->co
, delta
);
702 float d2
= v3_length2(delta
),
710 v3_muls( delta
, 1.0f
/d
, ct
->n
);
714 v3_muladds( c
, ct
->n
, -ra
, p0
);
715 v3_muladds( rbb
->co
, ct
->n
, rb
, p1
);
716 v3_add( p0
, p1
, ct
->co
);
717 v3_muls( ct
->co
, 0.5f
, ct
->co
);
728 static int rb_capsule_vs_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
730 float ha
= rba
->inf
.capsule
.height
,
731 hb
= rbb
->inf
.capsule
.height
,
732 ra
= rba
->inf
.capsule
.radius
,
733 rb
= rbb
->inf
.capsule
.radius
,
737 v3_muladds( rba
->co
, rba
->up
, -ha
*0.5f
+ra
, p0
);
738 v3_muladds( rba
->co
, rba
->up
, ha
*0.5f
-ra
, p1
);
739 v3_muladds( rbb
->co
, rbb
->up
, -hb
*0.5f
+rb
, p2
);
740 v3_muladds( rbb
->co
, rbb
->up
, hb
*0.5f
-rb
, p3
);
742 capsule_manifold manifold
;
743 rb_capsule_manifold_init( &manifold
);
747 closest_segment_segment( p0
, p1
, p2
, p3
, &ta
, &tb
, pa
, pb
);
748 rb_capsule_manifold( pa
, pb
, ta
, r
, &manifold
);
750 ta
= closest_point_segment( p0
, p1
, p2
, pa
);
751 tb
= closest_point_segment( p0
, p1
, p3
, pb
);
752 rb_capsule_manifold( pa
, p2
, ta
, r
, &manifold
);
753 rb_capsule_manifold( pb
, p3
, tb
, r
, &manifold
);
755 closest_point_segment( p2
, p3
, p0
, pa
);
756 closest_point_segment( p2
, p3
, p1
, pb
);
757 rb_capsule_manifold( p0
, pa
, 0.0f
, r
, &manifold
);
758 rb_capsule_manifold( p1
, pb
, 1.0f
, r
, &manifold
);
760 return rb_capsule_manifold_done( rba
, rbb
, &manifold
, buf
);
764 * Generates up to two contacts; optimised for the most stable manifold
766 static int rb_capsule_vs_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
768 float h
= rba
->inf
.capsule
.height
,
769 r
= rba
->inf
.capsule
.radius
;
772 * Solving this in symetric local space of the cube saves us some time and a
773 * couple branches when it comes to the quad stage.
776 v3_add( rbb
->bbx
[0], rbb
->bbx
[1], centroid
);
777 v3_muls( centroid
, 0.5f
, centroid
);
780 v3_sub( rbb
->bbx
[0], centroid
, bbx
[0] );
781 v3_sub( rbb
->bbx
[1], centroid
, bbx
[1] );
783 v3f pc
, p0w
, p1w
, p0
, p1
;
784 v3_muladds( rba
->co
, rba
->up
, -h
*0.5f
+r
, p0w
);
785 v3_muladds( rba
->co
, rba
->up
, h
*0.5f
-r
, p1w
);
787 m4x3_mulv( rbb
->to_local
, p0w
, p0
);
788 m4x3_mulv( rbb
->to_local
, p1w
, p1
);
789 v3_sub( p0
, centroid
, p0
);
790 v3_sub( p1
, centroid
, p1
);
791 v3_add( p0
, p1
, pc
);
792 v3_muls( pc
, 0.5f
, pc
);
795 * Finding an appropriate quad to collide lines with
798 v3_div( pc
, bbx
[1], region
);
801 if( (fabsf(region
[0]) > fabsf(region
[1])) &&
802 (fabsf(region
[0]) > fabsf(region
[2])) )
804 float px
= vg_signf(region
[0]) * bbx
[1][0];
805 v3_copy( (v3f
){ px
, bbx
[0][1], bbx
[0][2] }, quad
[0] );
806 v3_copy( (v3f
){ px
, bbx
[1][1], bbx
[0][2] }, quad
[1] );
807 v3_copy( (v3f
){ px
, bbx
[1][1], bbx
[1][2] }, quad
[2] );
808 v3_copy( (v3f
){ px
, bbx
[0][1], bbx
[1][2] }, quad
[3] );
810 else if( fabsf(region
[1]) > fabsf(region
[2]) )
812 float py
= vg_signf(region
[1]) * bbx
[1][1];
813 v3_copy( (v3f
){ bbx
[0][0], py
, bbx
[0][2] }, quad
[0] );
814 v3_copy( (v3f
){ bbx
[1][0], py
, bbx
[0][2] }, quad
[1] );
815 v3_copy( (v3f
){ bbx
[1][0], py
, bbx
[1][2] }, quad
[2] );
816 v3_copy( (v3f
){ bbx
[0][0], py
, bbx
[1][2] }, quad
[3] );
820 float pz
= vg_signf(region
[2]) * bbx
[1][2];
821 v3_copy( (v3f
){ bbx
[0][0], bbx
[0][1], pz
}, quad
[0] );
822 v3_copy( (v3f
){ bbx
[1][0], bbx
[0][1], pz
}, quad
[1] );
823 v3_copy( (v3f
){ bbx
[1][0], bbx
[1][1], pz
}, quad
[2] );
824 v3_copy( (v3f
){ bbx
[0][0], bbx
[1][1], pz
}, quad
[3] );
827 capsule_manifold manifold
;
828 rb_capsule_manifold_init( &manifold
);
831 closest_point_aabb( p0
, bbx
, c0
);
832 closest_point_aabb( p1
, bbx
, c1
);
835 v3_sub( c0
, p0
, d0
);
836 v3_sub( c1
, p1
, d1
);
837 v3_sub( p1
, p0
, da
);
844 if( v3_dot( da
, d0
) <= 0.01f
)
845 rb_capsule_manifold( p0
, c0
, 0.0f
, r
, &manifold
);
847 if( v3_dot( da
, d1
) >= -0.01f
)
848 rb_capsule_manifold( p1
, c1
, 1.0f
, r
, &manifold
);
850 for( int i
=0; i
<4; i
++ )
857 closest_segment_segment( p0
, p1
, quad
[i0
], quad
[i1
], &ta
, &tb
, ca
, cb
);
858 rb_capsule_manifold( ca
, cb
, ta
, r
, &manifold
);
862 * Create final contacts based on line manifold
864 m3x3_mulv( rbb
->to_world
, manifold
.d0
, manifold
.d0
);
865 m3x3_mulv( rbb
->to_world
, manifold
.d1
, manifold
.d1
);
872 for( int i
=0; i
<4; i
++ )
878 v3_add( quad
[i0
], centroid
, q0
);
879 v3_add( quad
[i1
], centroid
, q1
);
881 m4x3_mulv( rbb
->to_world
, q0
, q0
);
882 m4x3_mulv( rbb
->to_world
, q1
, q1
);
884 vg_line( q0
, q1
, 0xffffffff );
888 return rb_capsule_manifold_done( rba
, rbb
, &manifold
, buf
);
891 static int rb_sphere_vs_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
895 closest_point_obb( rba
->co
, rbb
, co
);
896 v3_sub( rba
->co
, co
, delta
);
898 float d2
= v3_length2(delta
),
899 r
= rba
->inf
.sphere
.radius
;
908 v3_sub( rba
->co
, rbb
->co
, delta
);
911 * some extra testing is required to find the best axis to push the
912 * object back outside the box. Since there isnt a clear seperating
913 * vector already, especially on really high aspect boxes.
915 float lx
= v3_dot( rbb
->right
, delta
),
916 ly
= v3_dot( rbb
->up
, delta
),
917 lz
= v3_dot( rbb
->forward
, delta
),
918 px
= rbb
->bbx
[1][0] - fabsf(lx
),
919 py
= rbb
->bbx
[1][1] - fabsf(ly
),
920 pz
= rbb
->bbx
[1][2] - fabsf(lz
);
922 if( px
< py
&& px
< pz
)
923 v3_muls( rbb
->right
, vg_signf(lx
), ct
->n
);
925 v3_muls( rbb
->up
, vg_signf(ly
), ct
->n
);
927 v3_muls( rbb
->forward
, vg_signf(lz
), ct
->n
);
929 v3_muladds( rba
->co
, ct
->n
, -r
, ct
->co
);
935 v3_muls( delta
, 1.0f
/d
, ct
->n
);
937 v3_copy( co
, ct
->co
);
948 static int rb_sphere_vs_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
951 v3_sub( rba
->co
, rbb
->co
, delta
);
953 float d2
= v3_length2(delta
),
954 r
= rba
->inf
.sphere
.radius
+ rbb
->inf
.sphere
.radius
;
961 v3_muls( delta
, 1.0f
/d
, ct
->n
);
964 v3_muladds( rba
->co
, ct
->n
,-rba
->inf
.sphere
.radius
, p0
);
965 v3_muladds( rbb
->co
, ct
->n
, rbb
->inf
.sphere
.radius
, p1
);
966 v3_add( p0
, p1
, ct
->co
);
967 v3_muls( ct
->co
, 0.5f
, ct
->co
);
977 /* TODO: these guys */
979 static int rb_capsule_vs_scene( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
983 int len
= bh_select( &rbb
->inf
.scene
.pscene
->bhtris
,
984 rba
->bbx_world
, geo
, 128 );
989 static int rb_sphere_vs_triangle( rigidbody
*rba
, rigidbody
*rbb
,
990 v3f tri
[3], rb_ct
*buf
)
994 closest_on_triangle( rba
->co
, tri
, co
);
995 v3_sub( rba
->co
, co
, delta
);
997 vg_line( rba
->co
, co
, 0xffff0000 );
998 vg_line_pt3( rba
->co
, 0.1f
, 0xff00ffff );
1000 float d2
= v3_length2( delta
),
1001 r
= rba
->inf
.sphere
.radius
;
1008 v3_sub( tri
[2], tri
[0], ab
);
1009 v3_sub( tri
[1], tri
[0], ac
);
1010 v3_cross( ac
, ab
, tn
);
1011 v3_copy( tn
, ct
->n
);
1012 v3_normalize( ct
->n
);
1014 float d
= sqrtf(d2
);
1016 v3_copy( co
, ct
->co
);
1026 static int rb_sphere_vs_scene( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1028 scene
*sc
= rbb
->inf
.scene
.pscene
;
1032 int len
= bh_select( &sc
->bhtris
, rba
->bbx_world
, geo
, 128 );
1036 for( int i
=0; i
<len
; i
++ )
1038 u32
*ptri
= &sc
->indices
[ geo
[i
]*3 ];
1040 for( int j
=0; j
<3; j
++ )
1041 v3_copy( sc
->verts
[ptri
[j
]].co
, tri
[j
] );
1043 vg_line(tri
[0],tri
[1],0xff00ff00 );
1044 vg_line(tri
[1],tri
[2],0xff00ff00 );
1045 vg_line(tri
[2],tri
[0],0xff00ff00 );
1047 buf
[count
].element_id
= ptri
[0];
1048 count
+= rb_sphere_vs_triangle( rba
, rbb
, tri
, buf
+count
);
1052 vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
1060 static float rb_box_plane_interval( rigidbody
*rba
, v4f p
)
1062 /* TODO: Make boxes COG aligned as is every other shape.
1063 * or create COG vector.
1064 * TODO: Make forward actually point in the right fucking direction. */
1066 v3_sub( rba
->bbx
[1], rba
->bbx
[0], e
);
1067 v3_muls( e
, 0.5f
, e
);
1068 v3_add( rba
->bbx
[0], e
, c
);
1069 m4x3_mulv( rba
->to_world
, c
, c
);
1072 e
[0]*fabsf( v3_dot(p
, rba
->right
)) +
1073 e
[1]*fabsf( v3_dot(p
, rba
->up
)) +
1074 e
[2]*fabsf(-v3_dot(p
, rba
->forward
)),
1075 s
= v3_dot( p
, c
) - p
[3];
1080 static int rb_box_vs_scene( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1083 scene
*sc
= rbb
->inf
.scene
.pscene
;
1087 int len
= bh_select( &sc
->bhtris
, rba
->bbx_world
, geo
, 128 );
1089 vg_info( "%d\n", len
);
1093 for( int i
=0; i
<len
; i
++ )
1095 u32
*ptri
= &sc
->indices
[ geo
[i
]*3 ];
1097 for( int j
=0; j
<3; j
++ )
1098 v3_copy( sc
->verts
[ptri
[j
]].co
, tri
[j
] );
1100 vg_line(tri
[0],tri
[1],0xff00ff00 );
1101 vg_line(tri
[1],tri
[2],0xff00ff00 );
1102 vg_line(tri
[2],tri
[0],0xff00ff00 );
1104 //count += rb_sphere_vs_triangle( rba, rbb, tri, buf+count );
1107 /* TODO: SAT test first */
1110 * each pair of faces on the box vs triangle normal
1116 v3_sub( tri
[1],tri
[0], v0
);
1117 v3_sub( tri
[2],tri
[0], v1
);
1118 v3_cross( v0
, v1
, tn
);
1121 tn
[3] = v3_dot( tn
, tri
[0] );
1123 v4f pa
, pb
, pc
, pd
, pe
, pf
;
1124 v3_copy( rba
->right
, pa
);
1125 v3_muls( rba
->right
, -1.0f
, pb
);
1126 v3_copy( rba
->up
, pc
);
1127 v3_muls( rba
->up
, -1.0f
, pd
);
1128 v3_copy( rba
->forward
, pf
);
1129 v3_muls( rba
->forward
, -1.0f
, pe
);
1131 float dx
= v3_dot( rba
->co
, rba
->right
),
1132 dy
= v3_dot( rba
->co
, rba
->up
),
1133 dz
= -v3_dot( rba
->co
, rba
->forward
);
1135 pa
[3] = dx
+ rba
->bbx
[1][0];
1136 pb
[3] = -dx
- rba
->bbx
[0][0];
1137 pc
[3] = dy
+ rba
->bbx
[1][1];
1138 pd
[3] = -dy
- rba
->bbx
[0][1];
1139 pe
[3] = dz
+ rba
->bbx
[1][2];
1140 pf
[3] = -dz
- rba
->bbx
[0][2];
1142 float *pairs
[][2] = { {pc
, pa
}, {pc
,pe
}, {pc
,pb
}, {pc
,pf
},
1143 {pd
, pa
}, {pd
,pe
}, {pd
,pb
}, {pd
,pf
},
1144 {pf
, pa
}, {pa
,pe
}, {pe
,pb
}, {pb
,pf
}};
1145 for( int j
=0; j
<vg_list_size(pairs
); j
++ )
1149 if( rb_intersect_planes_1( pairs
[j
][0], pairs
[j
][1], tn
, p
))
1152 closest_point_obb( p
, rba
, p_box
);
1153 closest_on_triangle_1( p
, tri
, p_tri
);
1155 //vg_line_pt3( p, 0.1f, 0xffeeaaff );
1157 if( v3_dist( p_tri
, p
) < 0.001f
&& v3_dist( p_box
, p
) < 0.001f
)
1161 vg_warn( "Exceeding box_vs_scene capacity."
1162 "Geometry too dense!\n" );
1166 rb_ct
*ct
= buf
+count
;
1168 v3_copy( tn
, ct
->n
);
1169 v3_copy( p_box
, ct
->co
);
1171 ct
->p
= rb_box_plane_interval( rba
, tn
);
1182 float *p000
= pts
[0], *p001
= pts
[1], *p010
= pts
[2], *p011
= pts
[3],
1183 *p100
= pts
[4], *p101
= pts
[5], *p110
= pts
[6], *p111
= pts
[7];
1185 p000
[0]=rba
->bbx
[0][0];p000
[1]=rba
->bbx
[0][1];p000
[2]=rba
->bbx
[0][2];
1186 p001
[0]=rba
->bbx
[0][0];p001
[1]=rba
->bbx
[0][1];p001
[2]=rba
->bbx
[1][2];
1187 p010
[0]=rba
->bbx
[0][0];p010
[1]=rba
->bbx
[1][1];p010
[2]=rba
->bbx
[0][2];
1188 p011
[0]=rba
->bbx
[0][0];p011
[1]=rba
->bbx
[1][1];p011
[2]=rba
->bbx
[1][2];
1189 p100
[0]=rba
->bbx
[1][0];p100
[1]=rba
->bbx
[0][1];p100
[2]=rba
->bbx
[0][2];
1190 p101
[0]=rba
->bbx
[1][0];p101
[1]=rba
->bbx
[0][1];p101
[2]=rba
->bbx
[1][2];
1191 p110
[0]=rba
->bbx
[1][0];p110
[1]=rba
->bbx
[1][1];p110
[2]=rba
->bbx
[0][2];
1192 p111
[0]=rba
->bbx
[1][0];p111
[1]=rba
->bbx
[1][1];p111
[2]=rba
->bbx
[1][2];
1195 for( int i
=0; i
<8; i
++ )
1197 m4x3_mulv( rba
->to_world
, pts
[i
], pts
[i
] );
1199 vg_line_pt3( pts
[i
], 0.1f
, 0xffff00ff );
1201 if( pts
[i
][1] < 0.0f
)
1203 rb_ct
*ct
= buf
+count
;
1205 v3_copy( (v3f
){0.0f
,1.0f
,0.0f
}, ct
->n
);
1206 v3_copy( pts
[i
], ct
->co
);
1208 ct
->p
= 0.0f
-pts
[i
][1];
1220 static int RB_MATRIX_ERROR( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1222 vg_error( "Collision type is unimplemented between types %d and %d\n",
1223 rba
->type
, rbb
->type
);
1228 static int rb_sphere_vs_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1230 return rb_capsule_vs_sphere( rbb
, rba
, buf
);
1233 static int rb_box_vs_capsule( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1235 return rb_capsule_vs_box( rbb
, rba
, buf
);
1238 static int rb_box_vs_sphere( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1240 return rb_sphere_vs_box( rbb
, rba
, buf
);
1243 static int rb_scene_vs_box( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1245 return rb_box_vs_scene( rbb
, rba
, buf
);
1248 static int (*rb_jump_table
[4][4])( rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1250 /* box */ /* Sphere */ /* Capsule */ /* Mesh */
1251 /*box */ { RB_MATRIX_ERROR
, rb_box_vs_sphere
, rb_box_vs_capsule
, rb_box_vs_scene
},
1252 /*sphere */ { rb_sphere_vs_box
, rb_sphere_vs_sphere
, rb_sphere_vs_capsule
, rb_sphere_vs_scene
},
1253 /*capsule*/ { rb_capsule_vs_box
,rb_capsule_vs_sphere
,rb_capsule_vs_capsule
,RB_MATRIX_ERROR
},
1254 /*mesh */ { rb_scene_vs_box
, RB_MATRIX_ERROR
, RB_MATRIX_ERROR
, RB_MATRIX_ERROR
}
1257 static int rb_collide( rigidbody
*rba
, rigidbody
*rbb
)
1259 int (*collider_jump
)(rigidbody
*rba
, rigidbody
*rbb
, rb_ct
*buf
)
1260 = rb_jump_table
[rba
->type
][rbb
->type
];
1263 * 12 is the maximum manifold size we can generate, so we are forced to abort
1264 * potentially checking any more.
1266 if( rb_contact_count
+ 12 > vg_list_size(rb_contact_buffer
) )
1268 vg_warn( "Too many contacts made in global collider buffer (%d of %d\n)",
1269 rb_contact_count
, vg_list_size(rb_contact_buffer
) );
1274 * TODO: Replace this with a more dedicated broad phase pass
1276 if( box_overlap( rba
->bbx_world
, rbb
->bbx_world
) )
1278 int count
= collider_jump( rba
, rbb
, rb_contact_buffer
+rb_contact_count
);
1279 rb_contact_count
+= count
;
1292 * This function does not accept triangle as a dynamic object, it is assumed
1293 * to always be static.
1295 * The triangle is also assumed to be one sided for better detection
1297 static int rb_sphere_vs_triangle( rigidbody
*rba
, v3f tri
[3], rb_ct
*buf
)
1301 closest_on_triangle( rba
->co
, tri
, co
);
1302 v3_sub( rba
->co
, co
, delta
);
1304 float d2
= v3_length2( delta
),
1305 r
= rba
->inf
.sphere
.radius
;
1310 v3_sub( tri
[1], tri
[0], ab
);
1311 v3_sub( tri
[2], tri
[0], ac
);
1312 v3_cross( ac
, ab
, tn
);
1314 if( v3_dot( delta
, tn
) > 0.0f
)
1315 v3_muls( delta
, -1.0f
, delta
);
1317 float d
= sqrtf(d2
);
1320 v3_muls( delta
, 1.0f
/d
, ct
->n
);
1321 v3_copy( co
, ct
->co
);
1324 ct
->rbb
= &rb_static_null
;
1331 static int sphere_vs_triangle( v3f c
, float r
, v3f tri
[3],
1332 v3f co
, v3f norm
, float *p
)
1335 closest_on_triangle( c
, tri
, co
);
1337 v3_sub( c
, co
, delta
);
1340 float d
= v3_length2( delta
);
1344 v3_sub( tri
[1], tri
[0], ab
);
1345 v3_sub( tri
[2], tri
[0], ac
);
1346 v3_cross( ac
, ab
, tn
);
1348 if( v3_dot( delta
, tn
) > 0.0f
)
1349 v3_muls( delta
, -1.0f
, delta
);
1351 vg_line_pt3( co
, 0.05f
, 0xff00ff00 );
1354 v3_muls( delta
, 1.0f
/d
, norm
);
1366 static void rb_solver_reset(void)
1368 rb_contact_count
= 0;
1371 static rb_ct
*rb_global_ct(void)
1373 return rb_contact_buffer
+ rb_contact_count
;
1377 static struct contact
*rb_start_contact(void)
1379 if( rb_contact_count
== vg_list_size(rb_contact_buffer
) )
1381 vg_error( "rigidbody: too many contacts generated (%u)\n",
1386 return &rb_contact_buffer
[ rb_contact_count
];
1389 static void rb_commit_contact( struct contact
*ct
, float p
)
1391 ct
->bias
= -0.2f
*k_rb_rate
*vg_minf(0.0f
,-p
+0.04f
);
1392 rb_tangent_basis( ct
->n
, ct
->t
[0], ct
->t
[1] );
1394 ct
->norm_impulse
= 0.0f
;
1395 ct
->tangent_impulse
[0] = 0.0f
;
1396 ct
->tangent_impulse
[1] = 0.0f
;
1398 rb_contact_count
++;
1401 static void rb_build_manifold_terrain_sphere( rigidbody
*rb
)
1405 int len
= bh_select( &world
.geo
.bhtris
, rb
->bbx_world
, geo
, 256 );
1407 for( int i
=0; i
<len
; i
++ )
1409 u32
*ptri
= &world
.geo
.indices
[ geo
[i
]*3 ];
1411 for( int j
=0; j
<3; j
++ )
1412 v3_copy( world
.geo
.verts
[ptri
[j
]].co
, tri
[j
] );
1414 vg_line(tri
[0],tri
[1],0xff00ff00 );
1415 vg_line(tri
[1],tri
[2],0xff00ff00 );
1416 vg_line(tri
[2],tri
[0],0xff00ff00 );
1421 for( int j
=0; j
<2; j
++ )
1423 if( sphere_vs_triangle( rb
->co
, rb
->inf
.sphere
.radius
, tri
,co
,norm
,&p
))
1425 struct contact
*ct
= rb_start_contact();
1431 v3_muladds( rb
->co
, norm
, p
, p1
);
1432 vg_line( rb
->co
, p1
, 0xffffffff );
1435 v3_copy( co
, ct
->co
);
1436 v3_copy( norm
, ct
->n
);
1437 rb_commit_contact( ct
, p
);
1444 static void rb_build_manifold_terrain( rigidbody
*rb
)
1448 float *p000
= pts
[0], *p001
= pts
[1], *p010
= pts
[2], *p011
= pts
[3],
1449 *p100
= pts
[4], *p101
= pts
[5], *p110
= pts
[6], *p111
= pts
[7];
1451 p000
[0]=box
[0][0];p000
[1]=box
[0][1];p000
[2]=box
[0][2];
1452 p001
[0]=box
[0][0];p001
[1]=box
[0][1];p001
[2]=box
[1][2];
1453 p010
[0]=box
[0][0];p010
[1]=box
[1][1];p010
[2]=box
[0][2];
1454 p011
[0]=box
[0][0];p011
[1]=box
[1][1];p011
[2]=box
[1][2];
1456 p100
[0]=box
[1][0];p100
[1]=box
[0][1];p100
[2]=box
[0][2];
1457 p101
[0]=box
[1][0];p101
[1]=box
[0][1];p101
[2]=box
[1][2];
1458 p110
[0]=box
[1][0];p110
[1]=box
[1][1];p110
[2]=box
[0][2];
1459 p111
[0]=box
[1][0];p111
[1]=box
[1][1];p111
[2]=box
[1][2];
1461 m4x3_mulv( rb
->to_world
, p000
, p000
);
1462 m4x3_mulv( rb
->to_world
, p001
, p001
);
1463 m4x3_mulv( rb
->to_world
, p010
, p010
);
1464 m4x3_mulv( rb
->to_world
, p011
, p011
);
1465 m4x3_mulv( rb
->to_world
, p100
, p100
);
1466 m4x3_mulv( rb
->to_world
, p101
, p101
);
1467 m4x3_mulv( rb
->to_world
, p110
, p110
);
1468 m4x3_mulv( rb
->to_world
, p111
, p111
);
1472 for( int i
=0; i
<8; i
++ )
1474 float *point
= pts
[i
];
1475 struct contact
*ct
= rb_start_contact();
1483 v3_copy( point
, surface
);
1487 hit
.dist
= INFINITY
;
1488 if( !ray_world( surface
, (v3f
){0.0f
,-1.0f
,0.0f
}, &hit
))
1491 v3_copy( hit
.pos
, surface
);
1493 float p
= vg_minf( surface
[1] - point
[1], 1.0f
);
1497 v3_copy( hit
.normal
, ct
->n
);
1498 v3_add( point
, surface
, ct
->co
);
1499 v3_muls( ct
->co
, 0.5f
, ct
->co
);
1501 rb_commit_contact( ct
, p
);
1511 * Initializing things like tangent vectors
1514 static void rb_presolve_contacts( rb_ct
*buffer
, int len
)
1516 for( int i
=0; i
<len
; i
++ )
1518 rb_ct
*ct
= &buffer
[i
];
1519 ct
->bias
= -0.2f
* k_rb_rate
* vg_minf(0.0f
,-ct
->p
+0.04f
);
1520 rb_tangent_basis( ct
->n
, ct
->t
[0], ct
->t
[1] );
1522 ct
->norm_impulse
= 0.0f
;
1523 ct
->tangent_impulse
[0] = 0.0f
;
1524 ct
->tangent_impulse
[1] = 0.0f
;
1525 ct
->mass_total
= 1.0f
/(ct
->rba
->inv_mass
+ ct
->rbb
->inv_mass
);
1527 rb_debug_contact( ct
);
1532 * Creates relative contact velocity vector, and offsets between each body
1534 static void rb_rcv( rb_ct
*ct
, v3f rv
, v3f da
, v3f db
)
1536 rigidbody
*rba
= ct
->rba
,
1539 v3_sub( ct
->co
, rba
->co
, da
);
1540 v3_sub( ct
->co
, rbb
->co
, db
);
1543 v3_cross( rba
->w
, da
, rva
);
1544 v3_add( rba
->v
, rva
, rva
);
1545 v3_cross( rbb
->w
, db
, rvb
);
1546 v3_add( rbb
->v
, rvb
, rvb
);
1548 v3_sub( rva
, rvb
, rv
);
1552 * Apply regular and angular velocity impulses to objects involved in contact
1554 static void rb_standard_impulse( rb_ct
*ct
, v3f da
, v3f db
, v3f impulse
)
1556 rigidbody
*rba
= ct
->rba
,
1560 v3_muls( impulse
, ct
->mass_total
*rba
->inv_mass
, ia
);
1561 v3_muls( impulse
, -ct
->mass_total
*rbb
->inv_mass
, ib
);
1564 v3_add( rba
->v
, ia
, rba
->v
);
1565 v3_add( rbb
->v
, ib
, rbb
->v
);
1567 /* Angular velocity */
1569 v3_cross( da
, ia
, wa
);
1570 v3_cross( db
, ib
, wb
);
1571 v3_add( rba
->w
, wa
, rba
->w
);
1572 v3_add( rbb
->w
, wb
, rbb
->w
);
1576 * One iteration to solve the contact constraint
1578 static void rb_solve_contacts( rb_ct
*buf
, int len
)
1580 float k_friction
= 0.5f
;
1582 /* Friction Impulse */
1583 for( int i
=0; i
<len
; i
++ )
1585 struct contact
*ct
= &buf
[i
];
1586 rigidbody
*rb
= ct
->rba
;
1589 rb_rcv( ct
, rv
, da
, db
);
1591 for( int j
=0; j
<2; j
++ )
1593 float f
= k_friction
* ct
->norm_impulse
,
1594 vt
= -v3_dot( rv
, ct
->t
[j
] );
1596 float temp
= ct
->tangent_impulse
[j
];
1597 ct
->tangent_impulse
[j
] = vg_clampf( temp
+vt
, -f
, f
);
1598 vt
= ct
->tangent_impulse
[j
] - temp
;
1601 v3_muls( ct
->t
[j
], vt
, impulse
);
1602 rb_standard_impulse( ct
, da
, db
, impulse
);
1606 /* Normal Impulse */
1607 for( int i
=0; i
<len
; i
++ )
1609 struct contact
*ct
= &buf
[i
];
1610 rigidbody
*rba
= ct
->rba
,
1614 rb_rcv( ct
, rv
, da
, db
);
1616 float vn
= -v3_dot( rv
, ct
->n
) + ct
->bias
;
1618 float temp
= ct
->norm_impulse
;
1619 ct
->norm_impulse
= vg_maxf( temp
+ vn
, 0.0f
);
1620 vn
= ct
->norm_impulse
- temp
;
1623 v3_muls( ct
->n
, vn
, impulse
);
1624 rb_standard_impulse( ct
, da
, db
, impulse
);
1629 * The following ventures into not really very sophisticated at all maths
1632 struct rb_angle_limit
1634 rigidbody
*rba
, *rbb
;
1636 float impulse
, bias
;
1639 static int rb_angle_limit_force( rigidbody
*rba
, v3f va
,
1640 rigidbody
*rbb
, v3f vb
,
1644 m3x3_mulv( rba
->to_world
, va
, wva
);
1645 m3x3_mulv( rbb
->to_world
, vb
, wvb
);
1647 float dt
= v3_dot(wva
,wvb
)*0.999f
,
1652 float correction
= max
-ang
;
1655 v3_cross( wva
, wvb
, axis
);
1658 q_axis_angle( rotation
, axis
, -correction
*0.25f
);
1659 q_mul( rotation
, rba
->q
, rba
->q
);
1661 q_axis_angle( rotation
, axis
, correction
*0.25f
);
1662 q_mul( rotation
, rbb
->q
, rbb
->q
);
1670 static void rb_constraint_angle_limit( struct rb_angle_limit
*limit
)
1675 static void rb_constraint_angle( rigidbody
*rba
, v3f va
,
1676 rigidbody
*rbb
, v3f vb
,
1677 float max
, float spring
)
1680 m3x3_mulv( rba
->to_world
, va
, wva
);
1681 m3x3_mulv( rbb
->to_world
, vb
, wvb
);
1683 float dt
= v3_dot(wva
,wvb
)*0.999f
,
1687 v3_cross( wva
, wvb
, axis
);
1688 v3_muladds( rba
->w
, axis
, ang
*spring
*0.5f
, rba
->w
);
1689 v3_muladds( rbb
->w
, axis
, -ang
*spring
*0.5f
, rbb
->w
);
1693 /* TODO: convert max into the dot product value so we dont have to always
1694 * evaluate acosf, only if its greater than the angle specified */
1698 float correction
= max
-ang
;
1701 q_axis_angle( rotation
, axis
, -correction
*0.125f
);
1702 q_mul( rotation
, rba
->q
, rba
->q
);
1704 q_axis_angle( rotation
, axis
, correction
*0.125f
);
1705 q_mul( rotation
, rbb
->q
, rbb
->q
);
1709 static void rb_relative_velocity( rigidbody
*ra
, v3f lca
,
1710 rigidbody
*rb
, v3f lcb
, v3f rcv
)
1713 m3x3_mulv( ra
->to_world
, lca
, wca
);
1714 m3x3_mulv( rb
->to_world
, lcb
, wcb
);
1716 v3_sub( ra
->v
, rb
->v
, rcv
);
1719 v3_cross( ra
->w
, wca
, rcv_Ra
);
1720 v3_cross( rb
->w
, wcb
, rcv_Rb
);
1721 v3_add( rcv_Ra
, rcv
, rcv
);
1722 v3_sub( rcv
, rcv_Rb
, rcv
);
1725 static void rb_constraint_position( rigidbody
*ra
, v3f lca
,
1726 rigidbody
*rb
, v3f lcb
)
1728 /* C = (COa + Ra*LCa) - (COb + Rb*LCb) = 0 */
1730 m3x3_mulv( ra
->to_world
, lca
, wca
);
1731 m3x3_mulv( rb
->to_world
, lcb
, wcb
);
1734 v3_add( wcb
, rb
->co
, delta
);
1735 v3_sub( delta
, wca
, delta
);
1736 v3_sub( delta
, ra
->co
, delta
);
1738 v3_muladds( ra
->co
, delta
, 0.5f
, ra
->co
);
1739 v3_muladds( rb
->co
, delta
, -0.5f
, rb
->co
);
1742 v3_sub( ra
->v
, rb
->v
, rcv
);
1745 v3_cross( ra
->w
, wca
, rcv_Ra
);
1746 v3_cross( rb
->w
, wcb
, rcv_Rb
);
1747 v3_add( rcv_Ra
, rcv
, rcv
);
1748 v3_sub( rcv
, rcv_Rb
, rcv
);
1750 float nm
= 0.5f
/(rb
->inv_mass
+ ra
->inv_mass
);
1752 float mass_a
= 1.0f
/ra
->inv_mass
,
1753 mass_b
= 1.0f
/rb
->inv_mass
,
1754 total_mass
= mass_a
+mass_b
;
1757 v3_muls( rcv
, 1.0f
, impulse
);
1758 v3_muladds( rb
->v
, impulse
, mass_b
/total_mass
, rb
->v
);
1759 v3_cross( wcb
, impulse
, impulse
);
1760 v3_add( impulse
, rb
->w
, rb
->w
);
1762 v3_muls( rcv
, -1.0f
, impulse
);
1763 v3_muladds( ra
->v
, impulse
, mass_a
/total_mass
, ra
->v
);
1764 v3_cross( wca
, impulse
, impulse
);
1765 v3_add( impulse
, ra
->w
, ra
->w
);
1769 * this could be used for spring joints
1770 * its not good for position constraint
1773 v3_muls( delta
, 0.5f
*spring
, impulse
);
1775 v3_add( impulse
, ra
->v
, ra
->v
);
1776 v3_cross( wca
, impulse
, impulse
);
1777 v3_add( impulse
, ra
->w
, ra
->w
);
1779 v3_muls( delta
, -0.5f
*spring
, impulse
);
1781 v3_add( impulse
, rb
->v
, rb
->v
);
1782 v3_cross( wcb
, impulse
, impulse
);
1783 v3_add( impulse
, rb
->w
, rb
->w
);
1787 static void debug_sphere( m4x3f m
, float radius
, u32 colour
)
1789 v3f ly
= { 0.0f
, 0.0f
, radius
},
1790 lx
= { 0.0f
, radius
, 0.0f
},
1791 lz
= { 0.0f
, 0.0f
, radius
};
1793 for( int i
=0; i
<16; i
++ )
1795 float t
= ((float)(i
+1) * (1.0f
/16.0f
)) * VG_PIf
* 2.0f
,
1799 v3f py
= { s
*radius
, 0.0f
, c
*radius
},
1800 px
= { s
*radius
, c
*radius
, 0.0f
},
1801 pz
= { 0.0f
, s
*radius
, c
*radius
};
1803 v3f p0
, p1
, p2
, p3
, p4
, p5
;
1804 m4x3_mulv( m
, py
, p0
);
1805 m4x3_mulv( m
, ly
, p1
);
1806 m4x3_mulv( m
, px
, p2
);
1807 m4x3_mulv( m
, lx
, p3
);
1808 m4x3_mulv( m
, pz
, p4
);
1809 m4x3_mulv( m
, lz
, p5
);
1811 vg_line( p0
, p1
, colour
== 0x00? 0xff00ff00: colour
);
1812 vg_line( p2
, p3
, colour
== 0x00? 0xff0000ff: colour
);
1813 vg_line( p4
, p5
, colour
== 0x00? 0xffff0000: colour
);
1821 static void debug_capsule( m4x3f m
, float radius
, float h
, u32 colour
)
1823 v3f ly
= { 0.0f
, 0.0f
, radius
},
1824 lx
= { 0.0f
, radius
, 0.0f
},
1825 lz
= { 0.0f
, 0.0f
, radius
};
1827 float s0
= sinf(0.0f
)*radius
,
1828 c0
= cosf(0.0f
)*radius
;
1830 v3f p0
, p1
, up
, right
, forward
;
1831 m3x3_mulv( m
, (v3f
){0.0f
,1.0f
,0.0f
}, up
);
1832 m3x3_mulv( m
, (v3f
){1.0f
,0.0f
,0.0f
}, right
);
1833 m3x3_mulv( m
, (v3f
){0.0f
,0.0f
,-1.0f
}, forward
);
1834 v3_muladds( m
[3], up
, -h
*0.5f
+radius
, p0
);
1835 v3_muladds( m
[3], up
, h
*0.5f
-radius
, p1
);
1838 v3_muladds( p0
, right
, radius
, a0
);
1839 v3_muladds( p1
, right
, radius
, a1
);
1840 v3_muladds( p0
, forward
, radius
, b0
);
1841 v3_muladds( p1
, forward
, radius
, b1
);
1842 vg_line( a0
, a1
, colour
);
1843 vg_line( b0
, b1
, colour
);
1845 v3_muladds( p0
, right
, -radius
, a0
);
1846 v3_muladds( p1
, right
, -radius
, a1
);
1847 v3_muladds( p0
, forward
, -radius
, b0
);
1848 v3_muladds( p1
, forward
, -radius
, b1
);
1849 vg_line( a0
, a1
, colour
);
1850 vg_line( b0
, b1
, colour
);
1852 for( int i
=0; i
<16; i
++ )
1854 float t
= ((float)(i
+1) * (1.0f
/16.0f
)) * VG_PIf
* 2.0f
,
1855 s1
= sinf(t
)*radius
,
1856 c1
= cosf(t
)*radius
;
1858 v3f e0
= { s0
, 0.0f
, c0
},
1859 e1
= { s1
, 0.0f
, c1
},
1860 e2
= { s0
, c0
, 0.0f
},
1861 e3
= { s1
, c1
, 0.0f
},
1862 e4
= { 0.0f
, c0
, s0
},
1863 e5
= { 0.0f
, c1
, s1
};
1865 m3x3_mulv( m
, e0
, e0
);
1866 m3x3_mulv( m
, e1
, e1
);
1867 m3x3_mulv( m
, e2
, e2
);
1868 m3x3_mulv( m
, e3
, e3
);
1869 m3x3_mulv( m
, e4
, e4
);
1870 m3x3_mulv( m
, e5
, e5
);
1872 v3_add( p0
, e0
, a0
);
1873 v3_add( p0
, e1
, a1
);
1874 v3_add( p1
, e0
, b0
);
1875 v3_add( p1
, e1
, b1
);
1877 vg_line( a0
, a1
, colour
);
1878 vg_line( b0
, b1
, colour
);
1882 v3_add( p0
, e2
, a0
);
1883 v3_add( p0
, e3
, a1
);
1884 v3_add( p0
, e4
, b0
);
1885 v3_add( p0
, e5
, b1
);
1889 v3_add( p1
, e2
, a0
);
1890 v3_add( p1
, e3
, a1
);
1891 v3_add( p1
, e4
, b0
);
1892 v3_add( p1
, e5
, b1
);
1895 vg_line( a0
, a1
, colour
);
1896 vg_line( b0
, b1
, colour
);
1903 static void rb_debug( rigidbody
*rb
, u32 colour
)
1905 if( rb
->type
== k_rb_shape_box
)
1908 vg_line_boxf_transformed( rb
->to_world
, rb
->bbx
, colour
);
1910 else if( rb
->type
== k_rb_shape_sphere
)
1912 debug_sphere( rb
->to_world
, rb
->inf
.sphere
.radius
, colour
);
1914 else if( rb
->type
== k_rb_shape_capsule
)
1917 float h
= rb
->inf
.capsule
.height
,
1918 r
= rb
->inf
.capsule
.radius
;
1920 debug_capsule( rb
->to_world
, r
, h
, colour
);
1922 else if( rb
->type
== k_rb_shape_scene
)
1924 vg_line_boxf( rb
->bbx
, colour
);
1930 * out penetration distance, normal
1932 static int rb_point_in_body( rigidbody
*rb
, v3f pos
, float *pen
, v3f normal
)
1935 m4x3_mulv( rb
->to_local
, pos
, local
);
1937 if( local
[0] > rb
->bbx
[0][0] && local
[0] < rb
->bbx
[1][0] &&
1938 local
[1] > rb
->bbx
[0][1] && local
[1] < rb
->bbx
[1][1] &&
1939 local
[2] > rb
->bbx
[0][2] && local
[2] < rb
->bbx
[1][2] )
1941 v3f area
, com
, comrel
;
1942 v3_add( rb
->bbx
[0], rb
->bbx
[1], com
);
1943 v3_muls( com
, 0.5f
, com
);
1945 v3_sub( rb
->bbx
[1], rb
->bbx
[0], area
);
1946 v3_sub( local
, com
, comrel
);
1947 v3_div( comrel
, area
, comrel
);
1950 float max_mag
= fabsf(comrel
[0]);
1952 if( fabsf(comrel
[1]) > max_mag
)
1955 max_mag
= fabsf(comrel
[1]);
1957 if( fabsf(comrel
[2]) > max_mag
)
1960 max_mag
= fabsf(comrel
[2]);
1964 normal
[axis
] = vg_signf(comrel
[axis
]);
1966 if( normal
[axis
] < 0.0f
)
1967 *pen
= local
[axis
] - rb
->bbx
[0][axis
];
1969 *pen
= rb
->bbx
[1][axis
] - local
[axis
];
1971 m3x3_mulv( rb
->to_world
, normal
, normal
);
1979 * BVH implementation, this is ONLY for static rigidbodies, its to slow for
1983 static void rb_bh_expand_bound( void *user
, boxf bound
, u32 item_index
)
1985 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
1986 box_concat( bound
, rb
->bbx_world
);
1989 static float rb_bh_centroid( void *user
, u32 item_index
, int axis
)
1991 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
1992 return (rb
->bbx_world
[axis
][0] + rb
->bbx_world
[1][axis
]) * 0.5f
;
1995 static void rb_bh_swap( void *user
, u32 ia
, u32 ib
)
1997 rigidbody temp
, *rba
, *rbb
;
1998 rba
= &((rigidbody
*)user
)[ ia
];
1999 rbb
= &((rigidbody
*)user
)[ ib
];
2006 static void rb_bh_debug( void *user
, u32 item_index
)
2008 rigidbody
*rb
= &((rigidbody
*)user
)[ item_index
];
2009 rb_debug( rb
, 0xff00ffff );
2012 static bh_system bh_system_rigidbodies
=
2014 .expand_bound
= rb_bh_expand_bound
,
2015 .item_centroid
= rb_bh_centroid
,
2016 .item_swap
= rb_bh_swap
,
2017 .item_debug
= rb_bh_debug
,
2023 #endif /* RIGIDBODY_H */