////////////////////////////////////////////////////////////////////////////////////////// // // 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); } }