//! return true if p is in the simplex s static bool isInside(const Point<N> &p, Simplex<N> &s) { bool inside = true; std::vector<Point<N> > opposed; float m1[N][N]; // x par rapport a l'arrete float m2[N][N]; // R par rapport a l'arrete for (int k = 0; k < s.getSize(); k++) { opposed.clear(); for (int i = 0; i < s.getSize(); i++) { if (i != k) opposed.push_back(s[i]); } // creation of matrix N*N for (size_t i = 0; i < N; i++) { for (size_t j = 0; j < N; j++) { m1[i][j] = p[i] - (opposed[j])[i]; // x par rapport a Fbi m2[i][j] = (s[k])[i] - (opposed[j])[i]; // R par rapport a Fbi } } if (Simplex<N>::determinant(m1, N) * Simplex<N>::determinant(m2, N) < 0) { // determinants de signes different inside = false; break; } } return inside; }
/** * Update the simplex and return closest point to origin on the simplex * @return Closest point to origin on the simplex */ template<class T> void GJKAlgorithm<T>::updateSimplex(Simplex<T> &simplex) const { Point3<T> closestPoint(0.0, 0.0, 0.0); T barycentrics[4]; if(simplex.getSize() == 2) { //simplex is a line (1D) const Point3<T> &pointA = simplex.getPoint(0); const Point3<T> &pointB = simplex.getPoint(1); //pointB is the last point added to the simplex closestPoint = LineSegment3D<T>(pointA, pointB).closestPoint(Point3<T>(0.0, 0.0, 0.0), barycentrics); simplex.setBarycentric(0, barycentrics[0]); simplex.setBarycentric(1, barycentrics[1]); }else if(simplex.getSize() == 3) { //simplex is a triangle (2D) const Point3<T> &pointA = simplex.getPoint(0); const Point3<T> &pointB = simplex.getPoint(1); const Point3<T> &pointC = simplex.getPoint(2); //pointC is the last point added to the simplex const Vector3<T> co = pointC.vector(Point3<T>(0.0, 0.0, 0.0)); const Vector3<T> cb = pointC.vector(pointB); const Vector3<T> ca = pointC.vector(pointA); const Vector3<T> normalAbc = cb.crossProduct(ca); closestPoint = Triangle3D<T>(pointA, pointB, pointC).closestPoint(Point3<T>(0.0, 0.0, 0.0), barycentrics); simplex.setBarycentric(0, barycentrics[0]); simplex.setBarycentric(1, barycentrics[1]); simplex.setBarycentric(2, barycentrics[2]); if(barycentrics[1]==0.0) { //remove pointB simplex.removePoint(1); } if(barycentrics[0]==0.0) { //remove pointA simplex.removePoint(0); } if(normalAbc.dotProduct(co) <= 0.0) { //voronoi region -ABC => ABC simplex.swapPoints(0, 1); //swap pointA and pointB } }else if (simplex.getSize() == 4) { //simplex is a tetrahedron (3D) const Point3<T> &pointA = simplex.getPoint(0); const Point3<T> &pointB = simplex.getPoint(1); const Point3<T> &pointC = simplex.getPoint(2); const Point3<T> &pointD = simplex.getPoint(3); //pointD is the last point added to the simplex const short voronoiRegionMask = 14; //test all voronoi regions except the one which doesn't include the new point added (pointD) closestPoint = Tetrahedron<T>(pointA, pointB, pointC, pointD).closestPoint(Point3<T>(0.0, 0.0, 0.0), barycentrics, voronoiRegionMask); simplex.setBarycentric(0, barycentrics[0]); simplex.setBarycentric(1, barycentrics[1]); simplex.setBarycentric(2, barycentrics[2]); simplex.setBarycentric(3, barycentrics[3]); if(barycentrics[2]==0.0) { //remove pointC simplex.removePoint(2); } if(barycentrics[1]==0.0) { //remove pointB simplex.removePoint(1); } if(barycentrics[0]==0.0) { //remove pointA simplex.removePoint(0); } }else { std::ostringstream oss; oss << simplex.getSize(); throw std::invalid_argument("Size of simplex unsupported: " + oss.str() + "."); } simplex.setClosestPointToOrigin(closestPoint); }