/******************************************************************************** * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * * Copyright (c) 2010-2013 Daniel Chappuis * ********************************************************************************* * * * This software is provided 'as-is', without any express or implied warranty. * * In no event will the authors be held liable for any damages arising from the * * use of this software. * * * * Permission is granted to anyone to use this software for any purpose, * * including commercial applications, and to alter it and redistribute it * * freely, subject to the following restrictions: * * * * 1. The origin of this software must not be misrepresented; you must not claim * * that you wrote the original software. If you use this software in a * * product, an acknowledgment in the product documentation would be * * appreciated but is not required. * * * * 2. Altered source versions must be plainly marked as such, and must not be * * misrepresented as being the original software. * * * * 3. This notice may not be removed or altered from any source distribution. * * * ********************************************************************************/ // Libraries #include "EPAAlgorithm.h" #include "../GJK/GJKAlgorithm.h" #include "TrianglesStore.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; // Constructor EPAAlgorithm::EPAAlgorithm(MemoryAllocator& memoryAllocator) : mMemoryAllocator(memoryAllocator) { } // Destructor 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 Vector3& p1, const Vector3& p2, const Vector3& p3, const Vector3& p4) const { // Check vertex 1 Vector3 normal1 = (p2-p1).cross(p3-p1); if (normal1.dot(p1) > 0.0 == normal1.dot(p4) > 0.0) { return 4; } // Check vertex 2 Vector3 normal2 = (p4-p2).cross(p3-p2); if (normal2.dot(p2) > 0.0 == normal2.dot(p1) > 0.0) { return 1; } // Check vertex 3 Vector3 normal3 = (p4-p3).cross(p1-p3); if (normal3.dot(p3) > 0.0 == normal3.dot(p2) > 0.0) { return 2; } // Check vertex 4 Vector3 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 algorithm. /// This method computes the penetration depth and contact points between two /// enlarged objects (with margin) where the original objects (without margin) /// intersect. An initial simplex that contains origin has been computed with /// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find /// the correct penetration depth bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex, CollisionShape* collisionShape1, const Transform& transform1, CollisionShape* collisionShape2, const Transform& transform2, Vector3& v, ContactPointInfo*& contactInfo) { Vector3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates Vector3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates Vector3 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 // Transform a point from local space of body 2 to local // space of body 1 (the GJK algorithm is done in local space of body 1) Transform body2Tobody1 = transform1.getInverse() * transform2; // Matrix that transform a direction from local // space of body 1 into local space of body 2 Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix(); // Get the simplex computed previously by the GJK algorithm unsigned int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points); // Compute the tolerance decimal tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint(); // Number of triangles in the polytope unsigned int nbTriangles = 0; // 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 with zero penetration depth. // We drop that kind of contact. Therefore, we return false 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 Vector3 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 decimal sin60 = decimal(sqrt(3.0)) * decimal(0.5); // Create a rotation quaternion to rotate the vector v1 to get the vectors // v2 and v3 Quaternion rotationQuat(d.x * sin60, d.y * sin60, d.z * sin60, 0.5); // Construct the corresponding rotation matrix Matrix3x3 rotationMat = rotationQuat.getMatrix(); // Compute the vector v1, v2, v3 Vector3 v1 = d.cross(Vector3(minAxis == 0, minAxis == 1, minAxis == 2)); Vector3 v2 = rotationMat * v1; Vector3 v3 = rotationMat * v2; // Compute the support point in the direction of v1 suppPointsA[2] = collisionShape1->getLocalSupportPointWithMargin(v1); suppPointsB[2] = body2Tobody1 * collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1)); points[2] = suppPointsA[2] - suppPointsB[2]; // Compute the support point in the direction of v2 suppPointsA[3] = collisionShape1->getLocalSupportPointWithMargin(v2); suppPointsB[3] = body2Tobody1 * collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2)); points[3] = suppPointsA[3] - suppPointsB[3]; // Compute the support point in the direction of v3 suppPointsA[4] = collisionShape1->getLocalSupportPointWithMargin(v3); suppPointsB[4] = body2Tobody1 * collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v3)); 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]; } // If the origin is in the tetrahedron of points 1, 2, 3, 4 else if (isOriginInTetrahedron(points[1], points[2], points[3], points[4]) == 0) { // We use the point 4 instead of point 0 for the initial tetrahedron suppPointsA[0] = suppPointsA[4]; suppPointsB[0] = suppPointsB[4]; points[0] = points[4]; } 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 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL) && 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 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(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST); 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 Vector3 v1 = points[1] - points[0]; Vector3 v2 = points[2] - points[0]; Vector3 n = v1.cross(v2); // Compute the two new vertices to obtain a hexahedron suppPointsA[3] = collisionShape1->getLocalSupportPointWithMargin(n); suppPointsB[3] = body2Tobody1 * collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * (-n)); points[3] = suppPointsA[3] - suppPointsB[3]; suppPointsA[4] = collisionShape1->getLocalSupportPointWithMargin(-n); suppPointsB[4] = body2Tobody1 * collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * n); 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 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL) && (face4 != NULL) && (face5 != NULL) && 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 link(EdgeEPA(face0, 1), EdgeEPA(face1, 2)); link(EdgeEPA(face1, 1), EdgeEPA(face2, 2)); link(EdgeEPA(face2, 1), EdgeEPA(face0, 2)); link(EdgeEPA(face0, 0), EdgeEPA(face5, 0)); 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 addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face4, triangleHeap, nbTriangles, DECIMAL_LARGEST); addFaceCandidate(face5, triangleHeap, nbTriangles, DECIMAL_LARGEST); 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; decimal upperBoundSquarePenDepth = DECIMAL_LARGEST; do { triangle = triangleHeap[0]; // Get the next candidate face (the face closest to the origin) std::pop_heap(&triangleHeap[0], &triangleHeap[nbTriangles], mTriangleComparison); 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] = collisionShape1->getLocalSupportPointWithMargin( triangle->getClosestPoint()); suppPointsB[nbVertices] = body2Tobody1 * collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * (-triangle->getClosestPoint())); points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices]; int indexNewVertex = nbVertices; nbVertices++; // Update the upper bound of the penetration depth decimal wDotv = points[indexNewVertex].dot(triangle->getClosestPoint()); assert(wDotv > 0.0); decimal wDotVSquare = wDotv * wDotv / triangle->getDistSquare(); if (wDotVSquare < upperBoundSquarePenDepth) { upperBoundSquarePenDepth = wDotVSquare; } // Compute the error decimal 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; } // Add all the new triangle faces computed with the silhouette algorithm // to the candidates list of faces of the current polytope 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 = transform1.getOrientation().getMatrix() * triangle->getClosestPoint(); Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA); Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB); Vector3 normal = v.getUnit(); decimal penetrationDepth = v.length(); assert(penetrationDepth > 0.0); // Create the contact info object contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo))) ContactPointInfo(normal, penetrationDepth, pALocal, pBLocal); return true; }