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::timeOfImpact(const PointSet & A, const PointSet & B, ContinuousCollisionContext * result)
{
result->hasContact = 0;
result->penetrateDepth = 0.f;
result->TOI = 0.f;
// std::cout<<"\nb test p"<<result->positionB;
// std::cout<<"\nb test v"<<result->linearVelocityB * 60.f;
// std::cout<<"\nb test w"<<result->angularVelocityB * 60.f;
const Vector3F relativeLinearVelocity = result->linearVelocityB - result->linearVelocityA;
// std::cout<<" velocityA "<<result->linearVelocityA.str();
// std::cout<<" velocityB "<<result->linearVelocityB.str();
// std::cout<<" relativeLinearVelocity "<<relativeLinearVelocity.str();
const float angularMotionSize = result->angularVelocityA.length() * A.angularMotionDisc()
+ result->angularVelocityB.length() * B.angularMotionDisc();
// no relative motion
if(relativeLinearVelocity.length() + angularMotionSize < TINY_VALUE)
return;
#ifdef DBG_DRAW
Vector3F lineB = result->positionB;
Vector3F lineE = lineB + relativeLinearVelocity;
glColor3f(0.f, .1f, .6f);
m_dbgDrawer->arrow(lineB, lineE);
lineB = result->positionA;
lineE = lineB - relativeLinearVelocity;
glColor3f(0.f, .1f, .6f);
m_dbgDrawer->arrow(lineB, lineE);
#endif
ClosestTestContext separateIo;
separateIo.needContributes = 1;
separateIo.margin = 0.05f;
Vector3F separateN;
float distance, closeInSpeed;
float lastDistance = 0.f;
float dDistanceaLamda;
const Vector3F position0A = result->positionA;
const Vector3F position0B = result->positionB;
const Quaternion orientation0A = result->orientationA;
const Quaternion orientation0B = result->orientationB;
float lamda = 0.f;
float limitDeltaLamda, deltaLamda = 1.f;
float lastLamda = 0.f;
int k = 0;
for(; k < 32; k++) {
separateIo.transformA.setTranslation(position0A.progress(result->linearVelocityA, lamda));
Quaternion ra = orientation0A.progress(result->angularVelocityA, lamda);
ra.normalize();
separateIo.transformA.setRotation(ra);
separateIo.transformB.setTranslation(position0B.progress(result->linearVelocityB, lamda));
Quaternion rb = orientation0B.progress(result->angularVelocityB, lamda);
rb.normalize();
separateIo.transformB.setRotation(rb);
// std::cout<<"\nk "<<k;
// std::cout<<"\nb at p"<<separateIo.transformB.getTranslation();
// std::cout<<"\nmat"<<separateIo.transformB.str();
separateIo.referencePoint.setZero();
separateIo.distance = 1e9;
separateDistance(A, B, &separateIo);
if(separateIo.hasResult) {
if(k<1) {
std::cout<<" contact at t0 try zero margin\n";
separateIo.margin = 0.f;
separateIo.distance = 1e9;
separateDistance(A, B, &separateIo);
if(separateIo.hasResult) {
std::cout<<" penetrated\n";
result->hasContact = 0;
return;
}
result->contactPointB = separateIo.contactPointB;
distance = separateIo.separateAxis.length();
result->penetrateDepth = 0.1 - distance;
separateN = separateIo.separateAxis / distance;
#ifdef DBG_GJK_DRAW
lineB = separateIo.transformB.transform(separateIo.contactPointB);
lineE = lineB + separateN;
glColor3f(1.f, 0.f, 0.f);
m_dbgDrawer->arrow(lineB, lineE);
#endif
break;
} else {
// std::cout<<" contact at "<<lamda;;
lamda = lastLamda;
break;
}
}
result->contactPointB = separateIo.contactPointB;
distance = separateIo.separateAxis.length();
if(distance < .001f) {
// std::cout<<" "<<k<<" close enough at "<<lamda<<"\n";
if(k<1) {
separateIo.margin = 0.f;
separateIo.distance = 1e9;
separateDistance(A, B, &separateIo);
result->contactPointB = separateIo.contactPointB;
distance = separateIo.separateAxis.length();
separateN = separateIo.separateAxis / distance;
}
break;
}
separateN = separateIo.separateAxis / distance;
#ifdef DBG_GJK_DRAW
lineB = separateIo.transformB.transform(separateIo.contactPointB);
lineE = lineB + separateN;
glColor3f(1.f, 0.f, 0.f);
m_dbgDrawer->arrow(lineB, lineE);
#endif
dDistanceaLamda = (distance - lastDistance) / deltaLamda;
lastDistance = distance;
// std::cout<<" sep ax "<<separateIo.separateAxis.str();
// std::cout<<" dist "<<distance;
// std::cout<<" sep n "<<separateN.str();
closeInSpeed = relativeLinearVelocity.dot(separateN);
// std::cout<<" closeInSpeed "<<closeInSpeed;
if(closeInSpeed + angularMotionSize < 0.f) {
// std::cout<<"go apart at time "<<lamda<<"\n";
return;
}
deltaLamda = distance / (closeInSpeed + angularMotionSize);
if(dDistanceaLamda < 0.f) {
limitDeltaLamda = -.59f * lastDistance / dDistanceaLamda;
if(deltaLamda > limitDeltaLamda) {
deltaLamda = limitDeltaLamda;
// std::cout<<" limit delta lamda "<<deltaLamda<<"\n";
}
}
lastLamda = lamda;
lamda += deltaLamda;
if(lamda < 0.f) {
// std::cout<<"lamda < 0\n";
return;
}
if(lamda > 1.f) {
// std::cout<<"lamda > 1\n";
return;
}
}
result->hasContact = 1;
result->TOI = lamda;
result->contactNormal = separateN.reversed();
// std::cout<<" v.n "<<result->contactNormal.dot(relativeLinearVelocity);
// std::cout<<" TOI "<<result->TOI;
// result->contactNormal.verbose(" N");
// relativeLinearVelocity.verbose(" Vrel");
#ifdef DBG_DRAW
lineB = separateIo.transformB.transform(separateIo.contactPointB);
lineE = lineB + result->contactNormal;
glColor3f(.2f, 1.f, .1f);
m_dbgDrawer->arrow(lineB, lineE);
#endif
}
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;
}