//////////////////////////////////////////////////////////////////////////////////////////
//
// RESOLVE CONTACT DATA USING BARYCENTRIC COORDINATES
//
////////////////////////////////////////////////////////////////////////////////////////////
Vec3f ResolveContact(Simplex& P){
/////////////////////////////////////////////////
// Barycentrics: Use them for A and B.
///////////////////////////////////////////////
assert(P.GetSize() > 0);
Vec3f contactA, contactB;
if(P.GetSize() == 1){
contactA = P.atA(0);
contactB = P.atB(0);
}else if(P.GetSize() == 2){//linear interpo;
Vec3f A = P.at(1);
Vec3f B = P.at(0);
Vec3f AB = B-A;
float v = (dot(AB, -A))/(dot(AB, -A)+ dot(-AB, -B));
contactA = P.atA(1)*(1-v) + P.atA(0)*v;
contactB = P.atB(1)*(1-v) + P.atB(0)*v;
}else{//triangle
Vec3f A = (P.at(2));
Vec3f B = (P.at(1));
Vec3f C = (P.at(0));
Vec3f N = cross(B-A,C-A);
Vec3f Nab = cross(N, B-A);
Vec3f Nac = cross(N, C-A);
float v = (dot(-A, Nac))/(dot(B-A,Nac));
float w = (dot(-A, Nab))/(dot(C-A, Nab));
float u = 1 - v - w;
contactA = P.atA(2)*u + P.atA(1)*v + P.atA(0)*w;
contactB = P.atB(2)*u + P.atB(1)*v + P.atB(0)*w;
}
Vec3f norm = contactB-contactA;
norm.normalize();
return contactA;
}
///////////////////////////////////////////////////////////////////
// CLOSEST IN
///////////////////////////////////////////////////////////////////
Vec3f ClosestIn(Simplex& P){
assert(P.GetSize() > 0);
if(P.GetSize() == 1) return P.at(0);
else if(P.GetSize() == 2){ //We have a line.
Vector3f AO(-P.at(1)[0], -P.at(1)[1], -P.at(1)[2]);
Vector3f AB(P.at(0)[0] - P.at(1)[0], P.at(0)[1] - P.at(1)[1], P.at(0)[2] - P.at(1)[2]);
Vector3f BO(-P.at(0)[0], -P.at(0)[1], -P.at(0)[2]);
float v = (AB.dot(AO))/(AB.dot(AO) + (-AB).dot(BO));
return (P.at(1)*(1-v) + (P.at(0))*v);
}else{//We have a triangle
Vector3f A(P.at(2)[0], P.at(2)[1], P.at(2)[2]);
Vector3f B(P.at(1)[0], P.at(1)[1], P.at(1)[2]);
Vector3f C (P.at(0)[0], P.at(0)[1], P.at(0)[2]);
Vector3f AB = B-A;
Vector3f AC = C-A;
Vector3f N = AB.cross(AC);
Vector3f Nab = N.cross(AB);
Vector3f Nac = N.cross(AC);
float v = (-A).dot(Nac)/((AB).dot(Nac));
float w = (-A).dot(Nab)/((AC).dot(Nab));
float u = 1-v-w;
return (P.at(2)*u + P.at(1)*v + P.at(0)*w);
}
}
