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