From cd5fda4396ab41b0206d5cd07c24e8157519bf2a Mon Sep 17 00:00:00 2001
From: "chappuis.daniel" <chappuis.daniel@92aac97c-a6ce-11dd-a772-7fcde58d38e6>
Date: Fri, 11 Feb 2011 14:51:09 +0000
Subject: [PATCH] implementation of GJK and EPA collision detection algorithm
continued
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@420 92aac97c-a6ce-11dd-a772-7fcde58d38e6
---
src/collision/EPA/EPAAlgorithm.cpp | 311 ++++++++++++++++++++++++++++-
src/collision/EPA/EPAAlgorithm.h | 38 +++-
src/collision/EPA/TriangleEPA.h | 4 +-
3 files changed, 345 insertions(+), 8 deletions(-)
diff --git a/src/collision/EPA/EPAAlgorithm.cpp b/src/collision/EPA/EPAAlgorithm.cpp
index 88f8e601..72ae50c4 100644
--- a/src/collision/EPA/EPAAlgorithm.cpp
+++ b/src/collision/EPA/EPAAlgorithm.cpp
@@ -24,6 +24,8 @@
// Libraries
#include "EPAAlgorithm.h"
+#include "../GJK/GJKAlgorithm.h"
+#include "TrianglesStore.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
@@ -38,6 +40,39 @@ EPAAlgorithm::~EPAAlgorithm() {
}
+// Decide if the origin is in the tetrahedron
+// Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of
+// the vertex that is wrong if the origin is not in the tetrahedron
+int EPAAlgorithm::isOriginInTetrahedron(const Vector3D& p1, const Vector3D& p2, const Vector3D& p3, const Vector3D& p4) const {
+
+ // Check vertex 1
+ Vector3D normal1 = (p2-p1).cross(p3-p1);
+ if (normal1.dot(p1) > 0.0 == normal1.dot(p4) > 0.0) {
+ return 4;
+ }
+
+ // Check vertex 2
+ Vector3D normal2 = (p4-p2).cross(p3-p2);
+ if (normal2.dot(p2) > 0.0 == normal2.dot(p1) > 0.0) {
+ return 1;
+ }
+
+ // Check vertex 3
+ Vector3D normal3 = (p4-p3).cross(p1-p3);
+ if (normal3.dot(p3) > 0.0 == normal3.dot(p2) > 0.0) {
+ return 2;
+ }
+
+ // Check vertex 4
+ Vector3D normal4 = (p2-p4).cross(p1-p4);
+ if (normal4.dot(p4) > 0.0 == normal4.dot(p3) > 0.0) {
+ return 3;
+ }
+
+ // The origin is in the tetrahedron, we return 0
+ return 0;
+}
+
// Compute the penetration depth with the EPA algorithms
// This method computes the penetration depth and contact points between two
// enlarged objects (with margin) where the original objects (without margin)
@@ -50,7 +85,9 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
Vector3D suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates
Vector3D suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
Vector3D points[MAX_SUPPORT_POINTS]; // Current points
-
+ TrianglesStore triangleStore; // Store the triangles
+ TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face candidate of the EPA algorithm
+
// TODO : Check that we call all the supportPoint() function with a margin
// Get the simplex computed previously by the GJK algorithm
@@ -62,7 +99,12 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
// Number of triangles in the polytope
unsigned int nbTriangles = 0;
- // Select an action according to the number of points in the simplex computed with GJK algorithm
+ // Clear the storing of triangles
+ triangleStore.clear();
+
+ // Select an action according to the number of points in the simplex
+ // computed with GJK algorithm in order to obtain an initial polytope for
+ // The EPA algorithm.
switch(nbVertices) {
case 1:
// Only one point in the simplex (which should be the origin). We have a touching contact
@@ -70,7 +112,270 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
return false;
case 2: {
-
+ // The simplex returned by GJK is a line segment d containing the origin.
+ // We add two additional support points to construct a hexahedron (two tetrahedron
+ // glued together with triangle faces. The idea is to compute three different vectors
+ // v1, v2 and v3 that are orthogonal to the segment d. The three vectors are relatively
+ // rotated of 120 degree around the d segment. The the three new points to
+ // construct the polytope are the three support points in those three directions
+ // v1, v2 and v3.
+
+ // Direction of the segment
+ Vector3D d = (points[1] - points[0]).getUnit();
+
+ // Choose the coordinate axis from the minimal absolute component of the vector d
+ int minAxis = d.getAbsoluteVector().getMinAxis();
+
+ // Compute sin(60)
+ const double sin60 = sqrt(3.0) * 0.5;
+
+ // Create a rotation quaternion to rotate the vector v1 to get the vectors
+ // v2 and v3
+ Quaternion rotationQuat(d.getX() * sin60, d.getY() * sin60, d.getZ() * sin60, 0.5);
+
+ // Construct the corresponding rotation matrix
+ Matrix3x3 rotationMat = rotationQuat.getMatrix();
+
+ // Compute the vector v1, v2, v3
+ Vector3D v1 = d.cross(Vector3D(minAxis == 0, minAxis == 1, minAxis == 2));
+ Vector3D v2 = rotationMat * v1;
+ Vector3D v3 = rotationMat * v2;
+
+ // Compute the support point in the direction of v1
+ suppPointsA[2] = boundingVolume1->getSupportPoint(v1, OBJECT_MARGIN);
+ suppPointsB[2] = boundingVolume2->getSupportPoint(v1.getOpposite(), OBJECT_MARGIN);
+ points[2] = suppPointsA[2] - suppPointsB[2];
+
+ // Compute the support point in the direction of v2
+ suppPointsA[3] = boundingVolume1->getSupportPoint(v2, OBJECT_MARGIN);
+ suppPointsB[3] = boundingVolume2->getSupportPoint(v2.getOpposite(), OBJECT_MARGIN);
+ points[3] = suppPointsA[3] - suppPointsB[3];
+
+ // Compute the support point in the direction of v3
+ suppPointsA[4] = boundingVolume1->getSupportPoint(v3, OBJECT_MARGIN);
+ suppPointsB[4] = boundingVolume2->getSupportPoint(v3.getOpposite(), OBJECT_MARGIN);
+ points[4] = suppPointsA[4] - suppPointsB[4];
+
+ // Now we have an hexahedron (two tetrahedron glued together). We can simply keep the
+ // tetrahedron that contains the origin in order that the initial polytope of the
+ // EPA algorithm is a tetrahedron, which is simpler to deal with.
+
+ // If the origin is in the tetrahedron of points 0, 2, 3, 4
+ if (isOriginInTetrahedron(points[0], points[2], points[3], points[4]) == 0) {
+ // We use the point 4 instead of point 1 for the initial tetrahedron
+ suppPointsA[1] = suppPointsA[4];
+ suppPointsB[1] = suppPointsB[4];
+ points[1] = points[4];
+ }
+ else if (isOriginInTetrahedron(points[1], points[2], points[3], points[4]) == 0) { // If the origin is in the tetrahedron of points 1, 2, 3, 4
+ // We use the point 4 instead of point 0 for the initial tetrahedron
+ suppPointsA[0] = suppPointsA[0];
+ suppPointsB[0] = suppPointsB[0];
+ points[0] = points[0];
+ }
+ else {
+ // The origin is not in the initial polytope
+ return false;
+ }
+
+ // The polytope contains now 4 vertices
+ nbVertices = 4;
}
+ case 4: {
+ // The simplex computed by the GJK algorithm is a tetrahedron. Here we check
+ // if this tetrahedron contains the origin. If it is the case, we keep it and
+ // otherwise we remove the wrong vertex of the tetrahedron and go in the case
+ // where the GJK algorithm compute a simplex of three vertices.
+
+ // Check if the tetrahedron contains the origin (or wich is the wrong vertex otherwise)
+ int badVertex = isOriginInTetrahedron(points[0], points[1], points[2], points[3]);
+
+ // If the origin is in the tetrahedron
+ if (badVertex == 0) {
+ // The tetrahedron is a correct initial polytope for the EPA algorithm.
+ // Therefore, we construct the tetrahedron.
+
+ // Comstruct the 4 triangle faces of the tetrahedron
+ TriangleEPA* face0 = triangleStore.newTriangle(points, 0, 1, 2);
+ TriangleEPA* face1 = triangleStore.newTriangle(points, 0, 3, 1);
+ TriangleEPA* face2 = triangleStore.newTriangle(points, 0, 2, 3);
+ TriangleEPA* face3 = triangleStore.newTriangle(points, 1, 3, 2);
+
+ // If the constructed tetrahedron is not correct
+ if (!(face0 && face1 && face2 && face3 && face0->getDistSquare() > 0.0 &&
+ face1->getDistSquare() > 0.0 && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
+ return false;
+ }
+
+ // Associate the edges of neighbouring triangle faces
+ EdgeEPA(face0, 0).link(EdgeEPA(face1, 2));
+ EdgeEPA(face0, 1).link(EdgeEPA(face3, 2));
+ EdgeEPA(face0, 2).link(EdgeEPA(face2, 0));
+ EdgeEPA(face1, 0).link(EdgeEPA(face2, 2));
+ EdgeEPA(face1, 1).link(EdgeEPA(face3, 0));
+ EdgeEPA(face2, 1).link(EdgeEPA(face3, 1));
+
+ // Add the triangle faces in the candidate heap
+ addFaceCandidate(face0, triangleHeap, nbTriangles, DBL_MAX);
+ addFaceCandidate(face1, triangleHeap, nbTriangles, DBL_MAX);
+ addFaceCandidate(face2, triangleHeap, nbTriangles, DBL_MAX);
+ addFaceCandidate(face3, triangleHeap, nbTriangles, DBL_MAX);
+
+ break;
+ }
+
+ // If the tetrahedron contains a wrong vertex (the origin is not inside the tetrahedron)
+ if (badVertex < 4) {
+ // Replace the wrong vertex with the point 5 (if it exists)
+ suppPointsA[badVertex-1] = suppPointsA[4];
+ suppPointsB[badVertex-1] = suppPointsB[4];
+ points[badVertex-1] = points[4];
+ }
+
+ // We have removed the wrong vertex
+ nbVertices = 3;
+ }
+ case 3: {
+ // The GJK algorithm returned a triangle that contains the origin.
+ // We need two new vertices to obtain a hexahedron. The two new vertices
+ // are the support points in the "n" and "-n" direction where "n" is the
+ // normal of the triangle.
+
+ // Compute the normal of the triangle
+ Vector3D v1 = points[1] - points[0];
+ Vector3D v2 = points[2] - points[0];
+ Vector3D n = v1.cross(v2);
+
+ // Compute the two new vertices to obtain a hexahedron
+ suppPointsA[3] = boundingVolume1->getSupportPoint(n, OBJECT_MARGIN);
+ suppPointsB[3] = boundingVolume2->getSupportPoint(n.getOpposite(), OBJECT_MARGIN);
+ points[3] = suppPointsA[3] - suppPointsB[3];
+ suppPointsA[4] = boundingVolume1->getSupportPoint(n.getOpposite(), OBJECT_MARGIN);
+ suppPointsB[4] = boundingVolume2->getSupportPoint(n, OBJECT_MARGIN);
+ points[4] = suppPointsA[4] - suppPointsB[4];
+
+ // Construct the triangle faces
+ TriangleEPA* face0 = triangleStore.newTriangle(points, 0, 1, 3);
+ TriangleEPA* face1 = triangleStore.newTriangle(points, 1, 2, 3);
+ TriangleEPA* face2 = triangleStore.newTriangle(points, 2, 0, 3);
+ TriangleEPA* face3 = triangleStore.newTriangle(points, 0, 2, 4);
+ TriangleEPA* face4 = triangleStore.newTriangle(points, 2, 1, 4);
+ TriangleEPA* face5 = triangleStore.newTriangle(points, 1, 0, 4);
+
+ // If the polytope hasn't been correctly constructed
+ if (!(face0 && face1 && face2 && face3 && face4 && face5 &&
+ face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 &&
+ face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0 &&
+ face4->getDistSquare() > 0.0 && face5->getDistSquare() > 0.0)) {
+ return false;
+ }
+
+ // Associate the edges of neighbouring faces
+ EdgeEPA(face0, 1).link(EdgeEPA(face1, 2));
+ EdgeEPA(face1, 1).link(EdgeEPA(face2, 2));
+ EdgeEPA(face2, 1).link(EdgeEPA(face0, 2));
+ EdgeEPA(face0, 0).link(EdgeEPA(face5, 0));
+ EdgeEPA(face1, 0).link(EdgeEPA(face4, 0));
+ EdgeEPA(face2, 0).link(EdgeEPA(face3, 0));
+ EdgeEPA(face3, 1).link(EdgeEPA(face4, 2));
+ EdgeEPA(face4, 1).link(EdgeEPA(face5, 2));
+ EdgeEPA(face5, 1).link(EdgeEPA(face3, 2));
+
+ // Add the candidate faces in the heap
+ addFaceCandidate(face0, triangleHeap, nbTriangles, DBL_MAX);
+ addFaceCandidate(face1, triangleHeap, nbTriangles, DBL_MAX);
+ addFaceCandidate(face2, triangleHeap, nbTriangles, DBL_MAX);
+ addFaceCandidate(face3, triangleHeap, nbTriangles, DBL_MAX);
+ addFaceCandidate(face4, triangleHeap, nbTriangles, DBL_MAX);
+ addFaceCandidate(face5, triangleHeap, nbTriangles, DBL_MAX);
+
+ nbVertices = 5;
+ }
+ break;
}
+
+ // At this point, we have a polytope that contains the origin. Therefore, we
+ // can run the EPA algorithm.
+
+ if (nbTriangles == 0) {
+ return false;
+ }
+
+ TriangleEPA* triangle = 0;
+ double upperBoundSquarePenDepth = DBL_MAX;
+
+ do {
+ triangle = triangleHeap[0];
+
+ // Get the next candidate face (the face closest to the origin)
+ std::pop_heap(&triangleHeap[0], &triangleHeap[nbTriangles], triangleComparison);
+ nbTriangles--;
+
+ // If the candidate face in the heap is not obsolete
+ if (!triangle->getIsObsolete()) {
+ // If we have reached the maximum number of support points
+ if (nbVertices == MAX_SUPPORT_POINTS) {
+ assert(false);
+ break;
+ }
+
+ // Compute the support point of the Minkowski difference (A-B) in the closest point direction
+ suppPointsA[nbVertices] = boundingVolume1->getSupportPoint(triangle->getClosestPoint(), OBJECT_MARGIN);
+ suppPointsB[nbVertices] = boundingVolume2->getSupportPoint(triangle->getClosestPoint().getOpposite());
+ points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
+
+ int indexNewVertex = nbVertices;
+ nbVertices++;
+
+ // Update the upper bound of the penetration depth
+ double wDotv = points[indexNewVertex].dot(triangle->getClosestPoint());
+ assert(wDotv > 0.0);
+ double wDotVSquare = wDotv * wDotv / triangle->getDistSquare();
+ if (wDotVSquare < upperBoundSquarePenDepth) {
+ upperBoundSquarePenDepth = wDotVSquare;
+ }
+
+ // Compute the error
+ double error = wDotv - triangle->getDistSquare();
+ if (error <= std::max(tolerance, REL_ERROR_SQUARE * wDotv) ||
+ points[indexNewVertex] == points[(*triangle)[0]] ||
+ points[indexNewVertex] == points[(*triangle)[1]] ||
+ points[indexNewVertex] == points[(*triangle)[2]]) {
+ break;
+ }
+
+ // Now, we compute the silhouette cast by the new vertex.
+ // The current triangle face will not be in the convex hull.
+ // We start the local recursive silhouette algorithm from
+ // the current triangle face.
+ int i = triangleStore.getNbTriangles();
+ if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) {
+ break;
+ }
+
+ // Construct the new polytope by constructing triangle faces from the
+ // silhouette to the new vertex of the polytope in order that the new
+ // polytope is always convex
+ while(i != triangleStore.getNbTriangles()) {
+ TriangleEPA* newTriangle = &triangleStore[i];
+
+ addFaceCandidate(newTriangle, triangleHeap, nbTriangles, upperBoundSquarePenDepth);
+ i++;
+ }
+ }
+
+ } while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
+
+ // Compute the contact info
+ v = triangle->getClosestPoint();
+ Vector3D pA = triangle->computeClosestPointOfObject(suppPointsA);
+ Vector3D pB = triangle->computeClosestPointOfObject(suppPointsB);
+ Vector3D diff = pB - pA;
+ Vector3D normal = diff.getUnit();
+ double penetrationDepth = diff.length();
+ assert(penetrationDepth > 0.0);
+ contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
+ normal, penetrationDepth, pA, pB);
+
+ return true;
}
diff --git a/src/collision/EPA/EPAAlgorithm.h b/src/collision/EPA/EPAAlgorithm.h
index 7d1afd72..28e90eed 100644
--- a/src/collision/EPA/EPAAlgorithm.h
+++ b/src/collision/EPA/EPAAlgorithm.h
@@ -30,6 +30,8 @@
#include "../../body/NarrowBoundingVolume.h"
#include "../ContactInfo.h"
#include "../../mathematics/mathematics.h"
+#include "TriangleEPA.h"
+#include <algorithm>
// ReactPhysics3D namespace
namespace reactphysics3d {
@@ -38,6 +40,20 @@ namespace reactphysics3d {
const unsigned int MAX_SUPPORT_POINTS = 100; // Maximum number of support points of the polytope
const unsigned int MAX_FACETS = 200; // Maximum number of facets of the polytope
+
+// Class TriangleComparison that allow the comparison of two triangles in the heap
+// The comparison between two triangles is made using their square distance to the closest
+// point to the origin. The goal is that in the heap, the first triangle is the one with the
+// smallest square distance.
+class TriangleComparison {
+ public:
+ // Comparison operator
+ bool operator()(const TriangleEPA* face1, const TriangleEPA* face2) {
+ return (face1->getDistSquare() > face2->getDistSquare());
+ }
+};
+
+
/* -------------------------------------------------------------------
Class EPAAlgorithm :
This class is the implementation of the Expanding Polytope Algorithm (EPA).
@@ -54,10 +70,12 @@ const unsigned int MAX_FACETS = 200; // Maximum number of facets of t
*/
class EPAAlgorithm {
private:
-
+ TriangleComparison triangleComparison; // Triangle comparison operator
- bool isOrigininInTetrahedron(const Vector3D& p1, const Vector3D& p2,
- const Vector3D& p3, const Vector3D& p4) const; // Return true if the origin is in the tetrahedron
+ void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap,
+ uint& nbTriangles, double upperBoundSquarePenDepth); // Add a triangle face in the candidate triangle heap
+ int isOriginInTetrahedron(const Vector3D& p1, const Vector3D& p2,
+ const Vector3D& p3, const Vector3D& p4) const; // Decide if the origin is in the tetrahedron
public:
EPAAlgorithm(); // Constructor
@@ -68,6 +86,20 @@ class EPAAlgorithm {
Vector3D& v, ContactInfo*& contactInfo); // Compute the penetration depth with EPA algorithm
};
+// Add a triangle face in the candidate triangle heap in the EPA algorithm
+inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap,
+ uint& nbTriangles, double upperBoundSquarePenDepth) {
+
+ // If the closest point of the affine hull of triangle points is internal to the triangle and
+ // if the distance of the closest point from the origin is at most the penetration depth upper bound
+ if (triangle->isClosestPointInternalToTriangle() && triangle->getDistSquare() <= upperBoundSquarePenDepth) {
+ // Add the triangle face to the list of candidates
+ heap[nbTriangles] = triangle;
+ nbTriangles++;
+ std::push_heap(&heap[0], &heap[nbTriangles], triangleComparison);
+ }
+}
+
} // End of ReactPhysics3D namespace
#endif
diff --git a/src/collision/EPA/TriangleEPA.h b/src/collision/EPA/TriangleEPA.h
index d0aaee67..49c227ad 100644
--- a/src/collision/EPA/TriangleEPA.h
+++ b/src/collision/EPA/TriangleEPA.h
@@ -118,8 +118,8 @@ inline bool TriangleEPA::isVisibleFromVertex(const Vector3D* vertices, uint inde
// Compute the point of an object closest to the origin
inline Vector3D TriangleEPA::computeClosestPointOfObject(const Vector3D* supportPointsOfObject) const {
const Vector3D& p0 = supportPointsOfObject[indicesVertices[0]];
- return p0 + (lambda1 * (supportPointsOfObject[indicesVertices[1]] - p0) +
- lambda2 * 1.0/det * (supportPointsOfObject[indicesVertices[2]] - p0));
+ return p0 + 1.0/det * (lambda1 * (supportPointsOfObject[indicesVertices[1]] - p0) +
+ lambda2 * (supportPointsOfObject[indicesVertices[2]] - p0));
}
// Access operator