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

implementation of GJK and EPA collision detection algorithm continued

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@424 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2011-05-18 21:34:39 +00:00
parent fce32c78e2
commit 867f940102
7 changed files with 78 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/body/BoundingSphere.h
View File

@ -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

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

@ -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;

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

@ -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;
}

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

@ -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;

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

@ -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;
}
}

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

@ -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

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

@ -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--;