From 867f9401027ebaef82be62622930e8e7f7ea6485 Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Wed, 18 May 2011 21:34:39 +0000 Subject: [PATCH] implementation of GJK and EPA collision detection algorithm continued git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@424 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- src/body/BoundingSphere.h | 4 +- src/collision/EPA/EPAAlgorithm.cpp | 64 ++++++++++++++++++------------ src/collision/EPA/EdgeEPA.cpp | 31 ++------------- src/collision/EPA/EdgeEPA.h | 3 -- src/collision/EPA/TriangleEPA.cpp | 26 +++++++++++- src/collision/EPA/TriangleEPA.h | 9 ++++- src/collision/EPA/TrianglesStore.h | 3 +- 7 files changed, 78 insertions(+), 62 deletions(-) diff --git a/src/body/BoundingSphere.h b/src/body/BoundingSphere.h index 05ff456b..1b8ae4f1 100644 --- a/src/body/BoundingSphere.h +++ b/src/body/BoundingSphere.h @@ -92,14 +92,14 @@ inline Vector3D BoundingSphere::getSupportPoint(const Vector3D& direction, doubl double length = direction.length(); // If the direction vector is not the zero vector - if (length > EPSILON) { + if (length > 0.0) { // Return the support point of the sphere in the given direction return center + (radius + margin) * direction.getUnit(); } // If the direction vector is the zero vector we return a point on the // boundary of the sphere - return center + Vector3D(radius + margin, 0.0, 0.0); + return center + Vector3D(0, radius + margin, 0); } }; // End of the ReactPhysics3D namespace diff --git a/src/collision/EPA/EPAAlgorithm.cpp b/src/collision/EPA/EPAAlgorithm.cpp index e44d9482..f19562fd 100644 --- a/src/collision/EPA/EPAAlgorithm.cpp +++ b/src/collision/EPA/EPAAlgorithm.cpp @@ -210,12 +210,12 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons } // 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)); + 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, DBL_MAX); @@ -252,18 +252,20 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons 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]; + suppPointsA[4] = boundingVolume1->getSupportPoint(n.getOpposite(), OBJECT_MARGIN); // TODO : Uncomment this + suppPointsB[4] = boundingVolume2->getSupportPoint(n, OBJECT_MARGIN); // TODO : Uncomment this + points[4] = suppPointsA[4] - suppPointsB[4]; // TODO : Uncomment this // 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); + //TriangleEPA* face3 = triangleStore.newTriangle(points, 0, 1, 2); // TODO : Remove this + TriangleEPA* face3 = triangleStore.newTriangle(points, 0, 2, 4);// TODO : Uncomment this + TriangleEPA* face4 = triangleStore.newTriangle(points, 2, 1, 4);// TODO : Uncomment this + TriangleEPA* face5 = triangleStore.newTriangle(points, 1, 0, 4);// TODO : Uncomment this + // TODO : Uncomment this // If the polytope hasn't been correctly constructed if (!(face0 && face1 && face2 && face3 && face4 && face5 && face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 && @@ -271,27 +273,38 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons face4->getDistSquare() > 0.0 && face5->getDistSquare() > 0.0)) { return false; } + + + // TODO : Remove this + /* + 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 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)); + 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, 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); + addFaceCandidate(face4, triangleHeap, nbTriangles, DBL_MAX); // TODO : Uncomment this + addFaceCandidate(face5, triangleHeap, nbTriangles, DBL_MAX); // TODO : Uncomment this - nbVertices = 5; + nbVertices = 5; // TODO : Uncomment this + //nbVertices = 4; // TODO : Remove this } break; } @@ -338,6 +351,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons } // Compute the error + double distSquare = triangle->getDistSquare(); // TODO : REMOVE THIS double error = wDotv - triangle->getDistSquare(); if (error <= std::max(tolerance, REL_ERROR_SQUARE * wDotv) || points[indexNewVertex] == points[(*triangle)[0]] || @@ -368,7 +382,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons } while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth); // Compute the contact info - v = triangle->getClosestPoint(); + v = triangle->getClosestPoint(); // TODO : Check if we have to compute this vector here Vector3D pA = triangle->computeClosestPointOfObject(suppPointsA); Vector3D pB = triangle->computeClosestPointOfObject(suppPointsB); Vector3D diff = pB - pA; diff --git a/src/collision/EPA/EdgeEPA.cpp b/src/collision/EPA/EdgeEPA.cpp index b420c308..3f421985 100644 --- a/src/collision/EPA/EdgeEPA.cpp +++ b/src/collision/EPA/EdgeEPA.cpp @@ -31,6 +31,7 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; + // Constructor EdgeEPA::EdgeEPA() { @@ -57,30 +58,6 @@ uint EdgeEPA::getTarget() const { return (*ownerTriangle)[indexOfNextCounterClockwiseEdge(index)]; } -// Link the edge with another one (meaning that the current edge of a triangle will -// is associated with the edge of another triangle in order that both triangles -// are neighbour along both edges) -bool EdgeEPA::link(EdgeEPA edge) { - bool isPossible = (this->getSource() == edge.getTarget() && this->getTarget() == edge.getSource()); - - // If the link is possible - if (isPossible) { - this->getOwnerTriangle()->setAdjacentEdge(index, edge); - edge.getOwnerTriangle()->setAdjacentEdge(edge.getIndex(), *this); - } - - // Return if the link has been made - return isPossible; -} - -// Half link the edge with another one from another triangle -void EdgeEPA::halfLink(EdgeEPA edge) { - assert(this->getSource() == edge.getTarget() && this->getTarget() == edge.getSource()); - - // Link - this->getOwnerTriangle()->setAdjacentEdge(index, edge); -} - // Compute the silhouette bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesStore& triangleStore) { // If the edge has not already been visited @@ -91,7 +68,7 @@ bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesS // If the triangle has been created if (triangle) { - EdgeEPA(triangle, 1).halfLink(*this); + halfLink(EdgeEPA(triangle, 1), *this); return true; } @@ -111,7 +88,7 @@ bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesS // If the triangle has been created if (triangle) { - EdgeEPA(triangle, 1).halfLink(*this); + halfLink(EdgeEPA(triangle, 1), *this); return true; } @@ -125,7 +102,7 @@ bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesS TriangleEPA* triangle = triangleStore.newTriangle(vertices, index, getTarget(), getSource()); if (triangle) { - EdgeEPA(triangle, 1).halfLink(*this); + halfLink(EdgeEPA(triangle, 1), *this); return true; } diff --git a/src/collision/EPA/EdgeEPA.h b/src/collision/EPA/EdgeEPA.h index 250f9a52..d1aede6b 100644 --- a/src/collision/EPA/EdgeEPA.h +++ b/src/collision/EPA/EdgeEPA.h @@ -56,13 +56,10 @@ class EdgeEPA { int getIndex() const; // Return the index of the edge in the triangle uint getSource() const; // Return index of the source vertex of the edge uint getTarget() const; // Return the index of the target vertex of the edge - bool link(EdgeEPA edge); // Link the edge with another one - void halfLink(EdgeEPA edge); // Half link the edge with another one bool computeSilhouette(const Vector3D* vertices, uint index, TrianglesStore& triangleStore); // Compute the recursive silhouette algorithm }; - // Return the pointer to the owner triangle inline TriangleEPA* EdgeEPA::getOwnerTriangle() const { return ownerTriangle; diff --git a/src/collision/EPA/TriangleEPA.cpp b/src/collision/EPA/TriangleEPA.cpp index 29d1e0a8..cca7ad5c 100644 --- a/src/collision/EPA/TriangleEPA.cpp +++ b/src/collision/EPA/TriangleEPA.cpp @@ -82,6 +82,28 @@ bool TriangleEPA::computeClosestPoint(const Vector3D* vertices) { return false; } +// Link an edge with another one (meaning that the current edge of a triangle will +// be associated with the edge of another triangle in order that both triangles +// are neighbour along both edges) +bool reactphysics3d::link(const EdgeEPA& edge0, const EdgeEPA& edge1) { + bool isPossible = (edge0.getSource() == edge1.getTarget() && edge0.getTarget() == edge1.getSource()); + + if (isPossible) { + edge0.getOwnerTriangle()->adjacentEdges[edge0.getIndex()] = edge1; + edge1.getOwnerTriangle()->adjacentEdges[edge1.getIndex()] = edge0; + } + + return isPossible; +} + +// Make an half link of an edge with another one from another triangle +void reactphysics3d::halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1) { + assert(edge0.getSource() == edge1.getTarget() && edge0.getTarget() == edge1.getSource()); + + // Link + edge0.getOwnerTriangle()->adjacentEdges[edge0.getIndex()] = edge1; +} + // Compute recursive silhouette algorithm for that triangle bool TriangleEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesStore& triangleStore) { @@ -100,9 +122,9 @@ bool TriangleEPA::computeSilhouette(const Vector3D* vertices, uint index, Triang int i,j; for (i=first, j=triangleStore.getNbTriangles()-1; i != triangleStore.getNbTriangles(); j = i++) { TriangleEPA* triangle = &triangleStore[i]; - triangle->getAdjacentEdge(1).halfLink(EdgeEPA(triangle, 1)); + halfLink(triangle->getAdjacentEdge(1), EdgeEPA(triangle, 1)); - if (!EdgeEPA(triangle, 0).link(EdgeEPA(&triangleStore[j], 2))) { + if (!link(EdgeEPA(triangle, 0), EdgeEPA(&triangleStore[j], 2))) { return false; } } diff --git a/src/collision/EPA/TriangleEPA.h b/src/collision/EPA/TriangleEPA.h index 029347f8..69fd7564 100644 --- a/src/collision/EPA/TriangleEPA.h +++ b/src/collision/EPA/TriangleEPA.h @@ -34,6 +34,11 @@ // ReactPhysics3D namespace namespace reactphysics3d { +// Prototypes +bool link(const EdgeEPA& edge0, const EdgeEPA& edge1); +void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1); + + /* ------------------------------------------------------------------- Class TriangleEPA : This class represents a triangle face of the current polytope in the EPA @@ -70,6 +75,8 @@ class TriangleEPA { TrianglesStore& triangleStore); // Compute recursive silhouette algorithm for that triangle uint operator[](int i) const; // Access operator + friend bool link(const EdgeEPA& edge0, const EdgeEPA& edge1); // Associate two edges + friend void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1); // Make a half-link between two edges }; // Return an edge of the triangle @@ -106,7 +113,7 @@ inline const Vector3D& TriangleEPA::getClosestPoint() const { // Return true if the closest point on affine hull is inside the triangle inline bool TriangleEPA::isClosestPointInternalToTriangle() const { - return (lambda1 >= 0.0 && lambda2 >= 0.0 && lambda1 + lambda2 <= det); + return (lambda1 >= 0.0 && lambda2 >= 0.0 && (lambda1 + lambda2) <= det); } // Return true if the triangle is visible from a given vertex diff --git a/src/collision/EPA/TrianglesStore.h b/src/collision/EPA/TrianglesStore.h index cf89eff4..d684f1ed 100644 --- a/src/collision/EPA/TrianglesStore.h +++ b/src/collision/EPA/TrianglesStore.h @@ -90,8 +90,7 @@ inline TriangleEPA* TrianglesStore::newTriangle(const Vector3D* vertices, uint v // If we have not reach the maximum number of triangles if (nbTriangles != MAX_TRIANGLES) { - newTriangle = &triangles[nbTriangles]; - nbTriangles++; + newTriangle = &triangles[nbTriangles++]; new (newTriangle) TriangleEPA(v0, v1, v2); if (!newTriangle->computeClosestPoint(vertices)) { nbTriangles--;