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

Continuation of EPA algorithm

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@426 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2011-06-08 21:22:47 +00:00
parent 436bdaf255
commit 23d0cd699f
7 changed files with 54 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
src/collision/EPA/EPAAlgorithm.cpp
View File

@ -30,8 +30,6 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// TODO : Check that allocated memory is correctly deleted
// Constructor
EPAAlgorithm::EPAAlgorithm() {
@ -89,8 +87,6 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
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
unsigned int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
@ -338,7 +334,6 @@ 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]] ||
@ -347,18 +342,16 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
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.
// 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
// 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);
@ -369,12 +362,11 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
} while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
// Compute the contact info
v = triangle->getClosestPoint(); // TODO : Check if we have to compute this vector here
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();
Vector3D normal = v.getUnit();
double penetrationDepth = v.length();
assert(penetrationDepth > 0.0);
contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(),
normal, penetrationDepth, pA, pB);

3
src/collision/EPA/EPAAlgorithm.h
View File

@ -65,7 +65,8 @@ class TriangleComparison {
that contains the origin and expend it in order to find the point on the boundary
of (A-B) that is closest to the origin. An initial simplex that contains origin
has been computed wit GJK algorithm. The EPA Algorithm will extend this simplex
polytope to find the correct penetration depth.
polytope to find the correct penetration depth. The implementation of the EPA
algorithm is based on the book "Collision Detection in 3D Environments".
-------------------------------------------------------------------
*/
class EPAAlgorithm {

20
src/collision/EPA/EdgeEPA.cpp
View File

@ -49,22 +49,22 @@ EdgeEPA::~EdgeEPA() {
}
// Return the index of the source vertex of the edge (vertex starting the edge)
uint EdgeEPA::getSource() const {
uint EdgeEPA::getSourceVertexIndex() const {
return (*ownerTriangle)[index];
}
// Return the index of the target vertex of the edge (vertex ending the edge)
uint EdgeEPA::getTarget() const {
uint EdgeEPA::getTargetVertexIndex() const {
return (*ownerTriangle)[indexOfNextCounterClockwiseEdge(index)];
}
// Compute the silhouette
bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesStore& triangleStore) {
// Execute the recursive silhouette algorithm from this edge
bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint indexNewVertex, TrianglesStore& triangleStore) {
// If the edge has not already been visited
if (!ownerTriangle->getIsObsolete()) {
// If the triangle of this edge is not visible from the given point
if (!ownerTriangle->isVisibleFromVertex(vertices, index)) {
TriangleEPA* triangle = triangleStore.newTriangle(vertices, index, getTarget(), getSource());
if (!ownerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) {
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex());
// If the triangle has been created
if (triangle) {
@ -80,10 +80,10 @@ bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesS
int backup = triangleStore.getNbTriangles();
if(!ownerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(this->index)).computeSilhouette(vertices, index, triangleStore)) {
if(!ownerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(this->index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) {
ownerTriangle->setIsObsolete(false);
TriangleEPA* triangle = triangleStore.newTriangle(vertices, index, getTarget(), getSource());
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex());
// If the triangle has been created
if (triangle) {
@ -93,12 +93,12 @@ bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesS
return false;
}
else if (!ownerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(this->index)).computeSilhouette(vertices, index, triangleStore)) {
else if (!ownerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(this->index)).computeSilhouette(vertices, indexNewVertex, triangleStore)) {
ownerTriangle->setIsObsolete(false);
triangleStore.setNbTriangles(backup);
TriangleEPA* triangle = triangleStore.newTriangle(vertices, index, getTarget(), getSource());
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex, getTargetVertexIndex(), getSourceVertexIndex());
if (triangle) {
halfLink(EdgeEPA(triangle, 1), *this);

6
src/collision/EPA/EdgeEPA.h
View File

@ -54,10 +54,10 @@ class EdgeEPA {
~EdgeEPA(); // Destructor
TriangleEPA* getOwnerTriangle() const; // Return the pointer to the owner triangle
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
uint getSourceVertexIndex() const; // Return index of the source vertex of the edge
uint getTargetVertexIndex() const; // Return the index of the target vertex of the edge
bool computeSilhouette(const Vector3D* vertices, uint index,
TrianglesStore& triangleStore); // Compute the recursive silhouette algorithm
TrianglesStore& triangleStore); // Execute the recursive silhouette algorithm from this edge
};
// Return the pointer to the owner triangle

33
src/collision/EPA/TriangleEPA.cpp
View File

@ -86,7 +86,8 @@ bool TriangleEPA::computeClosestPoint(const Vector3D* vertices) {
// 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());
bool isPossible = (edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() &&
edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex());
if (isPossible) {
edge0.getOwnerTriangle()->adjacentEdges[edge0.getIndex()] = edge1;
@ -96,30 +97,44 @@ bool reactphysics3d::link(const EdgeEPA& edge0, const EdgeEPA& edge1) {
return isPossible;
}
// Make an half link of an edge with another one from another triangle
// Make an half link of an edge with another one from another triangle. An half-link
// between an edge "edge0" and an edge "edge1" represents the fact that "edge1" is an
// adjacent edge of "edge0" but not the opposite. The opposite edge connection will
// be made later.
void reactphysics3d::halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1) {
assert(edge0.getSource() == edge1.getTarget() && edge0.getTarget() == edge1.getSource());
assert(edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() &&
edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex());
// Link
edge0.getOwnerTriangle()->adjacentEdges[edge0.getIndex()] = edge1;
}
// Compute recursive silhouette algorithm for that triangle
bool TriangleEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesStore& triangleStore) {
// Execute the recursive silhouette algorithm from this triangle face
// The parameter "vertices" is an array that contains the vertices of the current polytope and the
// parameter "indexNewVertex" is the index of the new vertex in this array. The goal of the silhouette algorithm is
// to add the new vertex in the polytope by keeping it convex. Therefore, the triangle faces that are visible from the
// new vertex must be removed from the polytope and we need to add triangle faces where each face contains the new vertex
// and an edge of the silhouette. The silhouette is the connected set of edges that are part of the border between faces that
// are seen and faces that are not seen from the new vertex. This method starts from the nearest face from the new vertex,
// computes the silhouette and create the new faces from the new vertex in order that we always have a convex polytope. The
// faces visible from the new vertex are set obselete and will not be considered as being a candidate face in the future.
bool TriangleEPA::computeSilhouette(const Vector3D* vertices, uint indexNewVertex, TrianglesStore& triangleStore) {
uint first = triangleStore.getNbTriangles();
// Mark the current triangle as obsolete
// Mark the current triangle as obsolete because it
setIsObsolete(true);
// Execute recursively the silhouette algorithm for the ajdacent edges of neighbouring
// triangles of the current triangle
bool result = adjacentEdges[0].computeSilhouette(vertices, index, triangleStore) &&
adjacentEdges[1].computeSilhouette(vertices, index, triangleStore) &&
adjacentEdges[2].computeSilhouette(vertices, index, triangleStore);
bool result = adjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
adjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
adjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore);
if (result) {
int i,j;
// For each triangle face that contains the new vertex and an edge of the silhouette
for (i=first, j=triangleStore.getNbTriangles()-1; i != triangleStore.getNbTriangles(); j = i++) {
TriangleEPA* triangle = &triangleStore[i];
halfLink(triangle->getAdjacentEdge(1), EdgeEPA(triangle, 1));

2
src/collision/EPA/TriangleEPA.h
View File

@ -72,7 +72,7 @@ class TriangleEPA {
bool computeClosestPoint(const Vector3D* vertices); // Compute the point v closest to the origin of this triangle
Vector3D computeClosestPointOfObject(const Vector3D* supportPointsOfObject) const; // Compute the point of an object closest to the origin
bool computeSilhouette(const Vector3D* vertices, uint index,
TrianglesStore& triangleStore); // Compute recursive silhouette algorithm for that triangle
TrianglesStore& triangleStore); // Execute the recursive silhouette algorithm from this triangle face
uint operator[](int i) const; // Access operator
friend bool link(const EdgeEPA& edge0, const EdgeEPA& edge1); // Associate two edges

13
src/collision/EPA/TrianglesStore.h
View File

@ -50,13 +50,13 @@ class TrianglesStore {
int nbTriangles; // Number of triangles
public:
TrianglesStore(); // Constructor
~TrianglesStore(); // Destructor
TrianglesStore(); // Constructor
~TrianglesStore(); // Destructor
void clear(); // Clear all the storage
int getNbTriangles() const; // Return the number of triangles
void setNbTriangles(int backup); // Set the number of triangles
TriangleEPA& last(); // Return the last triangle
void clear(); // Clear all the storage
int getNbTriangles() const; // Return the number of triangles
void setNbTriangles(int backup); // Set the number of triangles
TriangleEPA& last(); // Return the last triangle
TriangleEPA* newTriangle(const Vector3D* vertices, uint v0, uint v1, uint v2); // Create a new triangle
@ -94,7 +94,6 @@ inline TriangleEPA* TrianglesStore::newTriangle(const Vector3D* vertices, uint v
new (newTriangle) TriangleEPA(v0, v1, v2);
if (!newTriangle->computeClosestPoint(vertices)) {
nbTriangles--;
// TODO : DO WE HAVE TO add "delete newTriangle;" here ??
newTriangle = 0;
}
}