final Vector3 direction = v.normalize();
// final Vector3 midpoint = a.add(b).multiply(0.5);
final Vector3 midpoint = a.add(b).add(v.multiply(spb-spa)).multiply(0.5); // account for sphere sweeping
// contact space basis
final Matrix3 M = GramSchmidt.run(direction);
// make sure the normal direction is in the z-component
final Matrix3 B = new Matrix3(M.column(1),M.column(2),M.column(0));
// since B is orthogonal its inverse equals its transpose
final Matrix3 Binv = B.transpose();
// create a result handler for the intersection algorithm
final ORourke.ResultHandler handler = new ORourke.ResultHandler() {
public final void intersection(final Vector3 p, final Vector3 q) {
final ContactPoint cp = new ContactPoint();
cp.b1 = ga.getBody();
cp.b2 = gb.getBody();
cp.normal.assign(direction);
cp.point.assign( B.multiply(new Vector3(p.x,p.y,0)).add(midpoint) );
// distance along the z axis in contact space
cp.distance = (p.z-q.z)-spb-spa; // take into account sphere sweeping
// if contact is within the envelope size
if (cp.distance < envelope ) {
cp.depth = shell-cp.distance;
cp.envelope = envelope;
cp.restitution = restitution;
cp.friction = friction;
contacts.add(cp);
}
}
};
// apply transform to all points
for (Vector3 p: faceA) {
p.assign( Binv.multiply(p.sub(midpoint)));
}
for (Vector3 p: faceB) {
p.assign( Binv.multiply(p.sub(midpoint)));
}
// run 2d intersection
ORourke.run(faceA, faceB, handler);