+static int rb_capsule__capsule( m4x3f mtxA, rb_capsule *ca,
+ m4x3f mtxB, rb_capsule *cb, rb_ct *buf ){
+ float ha = ca->height,
+ hb = cb->height,
+ ra = ca->radius,
+ rb = cb->radius,
+ r = ra+rb;
+
+ v3f p0, p1, p2, p3;
+ v3_muladds( mtxA[3], mtxA[1], -ha*0.5f+ra, p0 );
+ v3_muladds( mtxA[3], mtxA[1], ha*0.5f-ra, p1 );
+ v3_muladds( mtxB[3], mtxB[1], -hb*0.5f+rb, p2 );
+ v3_muladds( mtxB[3], mtxB[1], hb*0.5f-rb, p3 );
+
+ capsule_manifold manifold;
+ rb_capsule_manifold_init( &manifold );
+
+ v3f pa, pb;
+ float ta, tb;
+ closest_segment_segment( p0, p1, p2, p3, &ta, &tb, pa, pb );
+ rb_capsule_manifold( pa, pb, ta, r, &manifold );
+
+ ta = closest_point_segment( p0, p1, p2, pa );
+ tb = closest_point_segment( p0, p1, p3, pb );
+ rb_capsule_manifold( pa, p2, ta, r, &manifold );
+ rb_capsule_manifold( pb, p3, tb, r, &manifold );
+
+ closest_point_segment( p2, p3, p0, pa );
+ closest_point_segment( p2, p3, p1, pb );
+ rb_capsule_manifold( p0, pa, 0.0f, r, &manifold );
+ rb_capsule_manifold( p1, pb, 1.0f, r, &manifold );
+
+ return rb_capsule__manifold_done( mtxA, ca, &manifold, buf );
+}
+
+static int rb_sphere_box( rb_object *obja, rb_object *objb, rb_ct *buf ){
+ v3f co, delta;
+ rigidbody *rba = &obja->rb, *rbb = &objb->rb;
+
+ closest_point_obb( rba->co, rbb->bbx, rbb->to_world, rbb->to_local, co );
+ v3_sub( rba->co, co, delta );
+
+ float d2 = v3_length2(delta),
+ r = obja->inf.sphere.radius;
+
+ if( d2 <= r*r ){
+ float d;
+
+ rb_ct *ct = buf;
+ if( d2 <= 0.0001f ){
+ v3_sub( rba->co, rbb->co, delta );
+
+ /*
+ * some extra testing is required to find the best axis to push the
+ * object back outside the box. Since there isnt a clear seperating
+ * vector already, especially on really high aspect boxes.
+ */
+ float lx = v3_dot( rbb->to_world[0], delta ),
+ ly = v3_dot( rbb->to_world[1], delta ),
+ lz = v3_dot( rbb->to_world[2], delta ),
+ px = rbb->bbx[1][0] - fabsf(lx),
+ py = rbb->bbx[1][1] - fabsf(ly),
+ pz = rbb->bbx[1][2] - fabsf(lz);
+
+ if( px < py && px < pz )
+ v3_muls( rbb->to_world[0], vg_signf(lx), ct->n );
+ else if( py < pz )
+ v3_muls( rbb->to_world[1], vg_signf(ly), ct->n );
+ else
+ v3_muls( rbb->to_world[2], vg_signf(lz), ct->n );
+
+ v3_muladds( rba->co, ct->n, -r, ct->co );
+ ct->p = r;
+ }
+ else{
+ d = sqrtf(d2);
+ v3_muls( delta, 1.0f/d, ct->n );
+ ct->p = r-d;
+ v3_copy( co, ct->co );
+ }