+static void rb_solver_reset(void);
+static void rb_build_manifold_terrain( rigidbody *rb );
+static void rb_build_manifold_terrain_sphere( rigidbody *rb );
+static void rb_solve_contacts(void);
+
+/*
+ * These closest point tests were learned from Real-Time Collision Detection by
+ * Christer Ericson
+ */
+static float closest_segment_segment( v3f p1, v3f q1, v3f p2, v3f q2,
+ float *s, float *t, v3f c1, v3f c2)
+{
+ v3f d1,d2,r;
+ v3_sub( q1, p1, d1 );
+ v3_sub( q2, p2, d2 );
+ v3_sub( p1, p2, r );
+
+ float a = v3_length2( d1 ),
+ e = v3_length2( d2 ),
+ f = v3_dot( d2, r );
+
+ const float kEpsilon = 0.0001f;
+
+ if( a <= kEpsilon && e <= kEpsilon )
+ {
+ *s = 0.0f;
+ *t = 0.0f;
+ v3_copy( p1, c1 );
+ v3_copy( p2, c2 );
+
+ v3f v0;
+ v3_sub( c1, c2, v0 );
+
+ return v3_length2( v0 );
+ }
+
+ if( a<= kEpsilon )
+ {
+ *s = 0.0f;
+ *t = vg_clampf( f / e, 0.0f, 1.0f );
+ }
+ else
+ {
+ float c = v3_dot( d1, r );
+ if( e <= kEpsilon )
+ {
+ *t = 0.0f;
+ *s = vg_clampf( -c / a, 0.0f, 1.0f );
+ }
+ else
+ {
+ float b = v3_dot(d1,d2),
+ d = a*e-b*b;
+
+ if( d != 0.0f )
+ {
+ *s = vg_clampf((b*f - c*e)/d, 0.0f, 1.0f);
+ }
+ else
+ {
+ *s = 0.0f;
+ }
+
+ *t = (b*(*s)+f) / e;
+
+ if( *t < 0.0f )
+ {
+ *t = 0.0f;
+ *s = vg_clampf( -c / a, 0.0f, 1.0f );
+ }
+ else if( *t > 1.0f )
+ {
+ *t = 1.0f;
+ *s = vg_clampf((b-c)/a,0.0f,1.0f);
+ }
+ }
+ }
+
+ v3_muladds( p1, d1, *s, c1 );
+ v3_muladds( p2, d2, *t, c2 );
+
+ v3f v0;
+ v3_sub( c1, c2, v0 );
+ return v3_length2( v0 );
+}
+
+static void closest_point_aabb( v3f p, boxf box, v3f dest )
+{
+ v3_maxv( p, box[0], dest );
+ v3_minv( dest, box[1], dest );
+}
+
+static void closest_point_obb( v3f p, rigidbody *rb, v3f dest )
+{
+ v3f local;
+ m4x3_mulv( rb->to_local, p, local );
+ closest_point_aabb( local, rb->bbx, local );
+ m4x3_mulv( rb->to_world, local, dest );
+}
+
+static void closest_point_segment( v3f a, v3f b, v3f point, v3f dest )
+{
+ v3f v0, v1;
+ v3_sub( b, a, v0 );
+ v3_sub( point, a, v1 );
+
+ float t = v3_dot( v1, v0 ) / v3_length2(v0);
+ v3_muladds( a, v0, vg_clampf(t,0.0f,1.0f), dest );
+}
+
+static void closest_on_triangle( v3f p, v3f tri[3], v3f dest )
+{
+ v3f ab, ac, ap;
+ float d1, d2;
+
+ /* Region outside A */
+ v3_sub( tri[1], tri[0], ab );
+ v3_sub( tri[2], tri[0], ac );
+ v3_sub( p, tri[0], ap );
+
+ d1 = v3_dot(ab,ap);
+ d2 = v3_dot(ac,ap);
+ if( d1 <= 0.0f && d2 <= 0.0f )
+ {
+ v3_copy( tri[0], dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Region outside B */
+ v3f bp;
+ float d3, d4;
+
+ v3_sub( p, tri[1], bp );
+ d3 = v3_dot( ab, bp );
+ d4 = v3_dot( ac, bp );
+
+ if( d3 >= 0.0f && d4 <= d3 )
+ {
+ v3_copy( tri[1], dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Edge region of AB */
+ float vc = d1*d4 - d3*d2;
+ if( vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f )
+ {
+ float v = d1 / (d1-d3);
+ v3_muladds( tri[0], ab, v, dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Region outside C */
+ v3f cp;
+ float d5, d6;
+ v3_sub( p, tri[2], cp );
+ d5 = v3_dot(ab, cp);
+ d6 = v3_dot(ac, cp);
+
+ if( d6 >= 0.0f && d5 <= d6 )
+ {
+ v3_copy( tri[2], dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Region of AC */
+ float vb = d5*d2 - d1*d6;
+ if( vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f )
+ {
+ float w = d2 / (d2-d6);
+ v3_muladds( tri[0], ac, w, dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* Region of BC */
+ float va = d3*d6 - d5*d4;
+ if( va <= 0.0f && (d4-d3) >= 0.0f && (d5-d6) >= 0.0f )
+ {
+ float w = (d4-d3) / ((d4-d3) + (d5-d6));
+ v3f bc;
+ v3_sub( tri[2], tri[1], bc );
+ v3_muladds( tri[1], bc, w, dest );
+ v3_copy( (v3f){INFINITY,INFINITY,INFINITY}, dest );
+ return;
+ }
+
+ /* P inside region, Q via barycentric coordinates uvw */
+ float d = 1.0f/(va+vb+vc),
+ v = vb*d,
+ w = vc*d;
+
+ v3_muladds( tri[0], ab, v, dest );
+ v3_muladds( dest, ac, w, dest );
+}
+
+/*
+ * Contact generators
+ *
+ * These do not automatically allocate contacts, an appropriately sized
+ * buffer must be supplied. The function returns the size of the manifold
+ * which was generated.
+ *
+ * The values set on the contacts are: n, co, p, rba, rbb
+ */
+
+static void rb_debug_contact( rb_ct *ct )
+{
+ v3f p1;
+ v3_muladds( ct->co, ct->n, 0.2f, p1 );
+ vg_line_pt3( ct->co, 0.1f, 0xff0000ff );
+ vg_line( ct->co, p1, 0xffffffff );
+}
+
+static int rb_sphere_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ v3f co, delta;
+
+ closest_point_obb( rba->co, rbb, co );
+ v3_sub( rba->co, co, delta );
+
+ float d2 = v3_length2(delta),
+ r = rba->inf.sphere.radius;
+
+ if( d2 <= r*r )
+ {
+ float d;
+ if( d2 <= 0.0001f )
+ {
+ v3_sub( rbb->co, rba->co, delta );
+ d2 = v3_length2(delta);
+ }
+
+ d = sqrtf(d2);
+
+ rb_ct *ct = buf;
+ v3_muls( delta, 1.0f/d, ct->n );
+ v3_copy( co, ct->co );
+ ct->p = r-d;
+ ct->rba = rba;
+ ct->rbb = rbb;
+ return 1;
+ }
+
+ return 0;
+}
+
+static int rb_sphere_vs_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ v3f delta;
+ v3_sub( rba->co, rbb->co, delta );
+
+ float d2 = v3_length2(delta),
+ r = rba->inf.sphere.radius + rbb->inf.sphere.radius;
+
+ if( d2 < r*r )
+ {
+ float d = sqrtf(d2);
+
+ rb_ct *ct = buf;
+ v3_muls( delta, -1.0f/d, ct->n );
+
+ v3f p0, p1;
+ v3_muladds( rba->co, ct->n, rba->inf.sphere.radius, p0 );
+ v3_muladds( rbb->co, ct->n,-rbb->inf.sphere.radius, p1 );
+ v3_add( p0, p1, ct->co );
+ v3_muls( ct->co, 0.5f, ct->co );
+ ct->p = r-d;
+ ct->rba = rba;
+ ct->rbb = rbb;
+ return 1;
+ }
+
+ return 0;
+}
+
+static int rb_box_vs_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ return rb_sphere_vs_box( rbb, rba, buf );
+}
+
+static int rb_box_vs_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
+{
+ /* TODO: Generating a stable quad manifold, lots of clipping */
+ return 0;
+}
+
+/*
+ * This function does not accept triangle as a dynamic object, it is assumed
+ * to always be static.
+ *
+ * The triangle is also assumed to be one sided for better detection
+ */
+static int rb_sphere_vs_triangle( rigidbody *rba, v3f tri[3], rb_ct *buf )
+{
+ v3f delta, co;
+
+ closest_on_triangle( rba->co, tri, co );
+ v3_sub( rba->co, co, delta );
+
+ float d2 = v3_length2( delta ),
+ r = rba->inf.sphere.radius;
+
+ if( d2 < r*r )
+ {
+ v3f ab, ac, tn;
+ v3_sub( tri[1], tri[0], ab );
+ v3_sub( tri[2], tri[0], ac );
+ v3_cross( ac, ab, tn );
+
+ if( v3_dot( delta, tn ) > 0.0f )
+ v3_muls( delta, -1.0f, delta );
+
+ float d = sqrtf(d2);
+
+ rb_ct *ct = buf;
+ v3_muls( delta, 1.0f/d, ct->n );
+ v3_copy( co, ct->co );
+ ct->p = r-d;
+ ct->rba = rba;
+ ct->rbb = &rb_static_null;
+ return 1;
+ }
+
+ return 0;
+}
+
+
+/*
+ * Generic functions
+ */
+
+RB_DEPR
+static int sphere_vs_triangle( v3f c, float r, v3f tri[3],
+ v3f co, v3f norm, float *p )
+{
+ v3f delta;
+ closest_on_triangle( c, tri, co );
+
+ v3_sub( c, co, delta );
+
+
+ float d = v3_length2( delta );
+ if( d < r*r )
+ {
+ v3f ab, ac, tn;
+ v3_sub( tri[1], tri[0], ab );
+ v3_sub( tri[2], tri[0], ac );
+ v3_cross( ac, ab, tn );
+
+ if( v3_dot( delta, tn ) > 0.0f )
+ v3_muls( delta, -1.0f, delta );
+
+ vg_line_pt3( co, 0.05f, 0xff00ff00 );
+
+ d = sqrtf(d);
+ v3_muls( delta, 1.0f/d, norm );
+
+ *p = r-d;
+ return 1;
+ }
+
+ return 0;
+}
+