void SetToSimplex(Simplex& s){
data.clear();
for(unsigned int i = 0; i< s.GetSize(); i++){
data.push_back(s[i]);
dataA.push_back(s.atA(i));
dataB.push_back(s.atB(i));
}
}
//////////////////////////////////////////////////////////////////////////////////////////
//
// 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;
}
