void GjkContactSolver::penetration(const PointSet & A, const PointSet & B, ClosestTestContext * result)
{
resetSimplex(result->W);
const Vector3F r = result->rayDirection;
const Vector3F startP = Vector3F::Zero - result->rayDirection * 99.f;
Vector3F hitP = startP;
// from origin to startP
Vector3F v = hitP;
Vector3F w, p, pa, pb, localA, localB;
float lamda = 0.f;
float vdotw, vdotr;
int k = 0;
for(; k < 39; k++) {
vdotr = v.dot(r);
// SA-B(v)
pa = A.supportPoint(v, result->transformA, localA, result->margin);
pb = B.supportPoint(v.reversed(), result->transformB, localB, result->margin);
p = pa - pb;// + v.normal() * MARGIN_DISTANCE;
w = hitP - p;
vdotw = v.dot(w);
if(vdotw > 0.f) {
if(vdotr >= 0.f)
break;
lamda -= vdotw / vdotr;
hitP = startP + r * lamda;
}
addToSimplex(result->W, p, localB);
result->hasResult = 0;
result->distance = 1e9;
result->referencePoint = hitP;
closestOnSimplex(result);
v = hitP - result->closestPoint;
interpolatePointB(result);
if(v.length2() < TINY_VALUE) break;
result->separateAxis = v;
smallestSimplex(result);
}
result->distance = hitP.length();
result->separateAxis.normalize();
}
void GjkContactSolver::separateDistance(const PointSet & A, const PointSet & B, ClosestTestContext * result)
{
resetSimplex(result->W);
float v2;
Vector3F w, pa, pb;
Vector3F v = result->transformA.transform(A.X[0]) - result->referencePoint;
if(v.length2() < TINY_VALUE) v = result->transformA.transform(A.X[1]) - result->referencePoint;
Vector3F localA, localB;
for(int i=0; i < 99; i++) {
// SA-B(-v)
pa = A.supportPoint(v.reversed(), result->transformA, localA, result->margin);
pb = B.supportPoint(v, result->transformB, localB, result->margin);
w = pa - pb;
// terminate when v is close enough to v(A - B).
// http://www.bulletphysics.com/ftp/pub/test/physics/papers/jgt04raycast.pdf
v2 = v.length2();
if(v2 - w.dot(v) < 0.0001f * v2) {
// std::cout<<" v is close to w "<<v2 - w.dot(v)<<"\n";
break;
}
addToSimplex(result->W, w, localB);
#ifdef DBG_GJK_DRAW
glPushMatrix();
m_dbgDrawer->useSpace(result->transformB);
glColor3f(0.f, .5f, 0.f);
m_dbgDrawer->arrow(Vector3F::Zero, localB);
glPopMatrix();
glPushMatrix();
m_dbgDrawer->useSpace(result->transformA);
glColor3f(.5f, 0.f, 0.f);
m_dbgDrawer->arrow(Vector3F::Zero, localA);
glPopMatrix();
glPushMatrix();
m_dbgDrawer->useSpace(result->transformA);
glColor3f(.5f, .5f, 0.f);
m_dbgDrawer->arrow(Vector3F::Zero, w + result->transformB.transform(localB));
glPopMatrix();
#endif
if(isPointInsideSimplex(result->W, result->referencePoint)) {
// std::cout<<" Minkowski difference contains the reference point\n";
result->hasResult = 1;
return;
}
result->hasResult = 0;
result->distance = 1e9;
closestOnSimplex(result);
v = result->closestPoint - result->referencePoint;
result->separateAxis = v;
#ifdef DBG_GJK_DRAW
glColor3f(.1f, 3.f, 0.f);
m_dbgDrawer->arrow(Vector3F::Zero, v);
#endif
interpolatePointB(result);
// in world space
smallestSimplex(result);
}
result->hasResult = 0;
}
void GjkContactSolver::rayCast(const PointSet & A, const PointSet & B, ClosestTestContext * result)
{
separateDistance(A, B, result);
if(result->hasResult) return;
resetSimplex(result->W);
const Vector3F r = result->rayDirection;
float lamda = 0.f;
// ray started at origin
const Vector3F startP = Vector3F::Zero;
Vector3F hitP = startP;
Vector3F hitN; hitN.setZero();
Vector3F v = hitP - result->closestPoint;
Vector3F w, p, pa, pb, localA, localB;
float vdotw, vdotr;
int k = 0;
for(; k < 32; k++) {
vdotr = v.dot(r);
// SA-B(v)
pa = A.supportPoint(v, result->transformA, localA, result->margin);
pb = B.supportPoint(v.reversed(), result->transformB, localB, result->margin);
p = pa - pb;
w = hitP - p;
vdotw = v.dot(w);
if(vdotw > 0.f) {
// std::cout<<" v.w > 0\n";
if(vdotr >= 0.f) {
// std::cout<<" v.r >= 0 missed\n";
result->hasResult = 0;
return;
}
lamda -= vdotw / vdotr;
hitP = startP + r * lamda;
hitN = v;
}
addToSimplex(result->W, p, localB);
result->hasResult = 0;
result->distance = 1e9;
result->referencePoint = hitP;
closestOnSimplex(result);
v = hitP - result->closestPoint;
interpolatePointB(result);
if(v.length2() < TINY_VALUE) break;
smallestSimplex(result);
}
if(k==32) std::cout<<" max iterations reached!\n";
// std::cout<<" k"<<k<<" ||v|| "<<v.length()<<"\n";
result->hasResult = 1;
result->separateAxis = hitN.normal();
result->distance = lamda;
}