mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix two issues in EPA Algorithm

Browse Source
This commit is contained in:
Daniel Chappuis 2015-01-18 18:56:44 +01:00
parent c8a83768d5
commit 54d8b8518e
1 changed files with 57 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

92
src/collision/narrowphase/EPA/EPAAlgorithm.cpp
View File

@ -27,6 +27,7 @@
#include "EPAAlgorithm.h" #include "EPAAlgorithm.h"
#include "collision/narrowphase//GJK/GJKAlgorithm.h" #include "collision/narrowphase//GJK/GJKAlgorithm.h"
#include "TrianglesStore.h" #include "TrianglesStore.h"
#include <iostream> // TODO : DELETE THIS
// We want to use the ReactPhysics3D namespace // We want to use the ReactPhysics3D namespace
using namespace reactphysics3d; using namespace reactphysics3d;
@ -245,13 +246,14 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
break; break;
} }
// If the tetrahedron contains a wrong vertex (the origin is not inside the tetrahedron) // The tetrahedron contains a wrong vertex (the origin is not inside the tetrahedron)
// Remove the wrong vertex and continue to the next case with the
// three remaining vertices
if (badVertex < 4) { if (badVertex < 4) {
// Replace the wrong vertex with the point 5 (if it exists) suppPointsA[badVertex-1] = suppPointsA[3];
suppPointsA[badVertex-1] = suppPointsA[4]; suppPointsB[badVertex-1] = suppPointsB[3];
suppPointsB[badVertex-1] = suppPointsB[4]; points[badVertex-1] = points[3];
points[badVertex-1] = points[4];
} }
// We have removed the wrong vertex // We have removed the wrong vertex
@ -259,9 +261,10 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
} }
case 3: { case 3: {
// The GJK algorithm returned a triangle that contains the origin. // The GJK algorithm returned a triangle that contains the origin.
// We need two new vertices to obtain a hexahedron. The two new vertices // We need two new vertices to create two tetrahedron. The two new
// are the support points in the "n" and "-n" direction where "n" is the // vertices are the support points in the "n" and "-n" direction
// normal of the triangle. // where "n" is the normal of the triangle. Then, we use only the
// tetrahedron that contains the origin.
// Compute the normal of the triangle // Compute the normal of the triangle
Vector3 v1 = points[1] - points[0]; Vector3 v1 = points[1] - points[0];
@ -278,43 +281,62 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * n); collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * n);
points[4] = suppPointsA[4] - suppPointsB[4]; points[4] = suppPointsA[4] - suppPointsB[4];
// Construct the triangle faces TriangleEPA* face0;
TriangleEPA* face0 = triangleStore.newTriangle(points, 0, 1, 3); TriangleEPA* face1;
TriangleEPA* face1 = triangleStore.newTriangle(points, 1, 2, 3); TriangleEPA* face2;
TriangleEPA* face2 = triangleStore.newTriangle(points, 2, 0, 3); TriangleEPA* face3;
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 the origin is in the first tetrahedron
if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL) if (isOriginInTetrahedron(points[0], points[1],
&& (face4 != NULL) && (face5 != NULL) && points[2], points[3]) == 0) {
face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 && // The tetrahedron is a correct initial polytope for the EPA algorithm.
face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0 && // Therefore, we construct the tetrahedron.
face4->getDistSquare() > 0.0 && face5->getDistSquare() > 0.0)) {
// Comstruct the 4 triangle faces of the tetrahedron
face0 = triangleStore.newTriangle(points, 0, 1, 2);
face1 = triangleStore.newTriangle(points, 0, 3, 1);
face2 = triangleStore.newTriangle(points, 0, 2, 3);
face3 = triangleStore.newTriangle(points, 1, 3, 2);
}
else if (isOriginInTetrahedron(points[0], points[1],
points[2], points[4]) == 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
face0 = triangleStore.newTriangle(points, 0, 1, 2);
face1 = triangleStore.newTriangle(points, 0, 4, 1);
face2 = triangleStore.newTriangle(points, 0, 2, 4);
face3 = triangleStore.newTriangle(points, 1, 4, 2);
}
else {
return false; return false;
} }
// Associate the edges of neighbouring faces // If the constructed tetrahedron is not correct
link(EdgeEPA(face0, 1), EdgeEPA(face1, 2)); if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL)
link(EdgeEPA(face1, 1), EdgeEPA(face2, 2)); && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0
link(EdgeEPA(face2, 1), EdgeEPA(face0, 2)); && face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
link(EdgeEPA(face0, 0), EdgeEPA(face5, 0)); return false;
link(EdgeEPA(face1, 0), EdgeEPA(face4, 0)); }
link(EdgeEPA(face2, 0), EdgeEPA(face3, 0));
link(EdgeEPA(face3, 1), EdgeEPA(face4, 2));
link(EdgeEPA(face4, 1), EdgeEPA(face5, 2));
link(EdgeEPA(face5, 1), EdgeEPA(face3, 2));
// Add the candidate faces in the heap // Associate the edges of neighbouring triangle faces
link(EdgeEPA(face0, 0), EdgeEPA(face1, 2));
link(EdgeEPA(face0, 1), EdgeEPA(face3, 2));
link(EdgeEPA(face0, 2), EdgeEPA(face2, 0));
link(EdgeEPA(face1, 0), EdgeEPA(face2, 2));
link(EdgeEPA(face1, 1), EdgeEPA(face3, 0));
link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
// Add the triangle faces in the candidate heap
addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face4, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face5, triangleHeap, nbTriangles, DECIMAL_LARGEST);
nbVertices = 5; nbVertices = 4;
} }
break; break;
} }