Vector3F AccCorner::computeNormal() const
{
if(isOnBoundary())
return *_centerNormal * (2.f / 3.f) + _edgeNormals[0] * (1.f / 6.f) + _edgeNormals[valence() - 1] * (1.f / 6.f);
float e = 4.f;
float c = 1.f;
float sum = 0.f;
Vector3F res;
res.setZero();
Vector3F q;
for(int i = 0; i < valence(); i++) {
q = _edgeNormals[i];
res += q * e;
sum += e;
q = _cornerNormals[i];
res += q * c;
sum += c;
}
sum += valence() * valence();
res += *_centerNormal * valence() * valence();
return res / sum;
}
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;
}