void Plane::set(const Vector3F & nor, const Vector3F & pop)
{
Vector3F nn = nor.normal();
m_a = nn.x;
m_b = nn.y;
m_c = nn.z;
m_d = - pop.dot(nn);
}
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;
}
float Vector3F::angleBetween(const Vector3F& another, const Vector3F& up) const
{
float ang = acos(this->normal().dot(another.normal()));
if(another.dot(up) >= 0.f) return ang;
return -ang;
}