implementation of GJK and EPA collision detection algorithm continued
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@424 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
parent
fce32c78e2
commit
867f940102
|
@ -92,14 +92,14 @@ inline Vector3D BoundingSphere::getSupportPoint(const Vector3D& direction, doubl
|
||||||
double length = direction.length();
|
double length = direction.length();
|
||||||
|
|
||||||
// If the direction vector is not the zero vector
|
// 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 the support point of the sphere in the given direction
|
||||||
return center + (radius + margin) * direction.getUnit();
|
return center + (radius + margin) * direction.getUnit();
|
||||||
}
|
}
|
||||||
|
|
||||||
// If the direction vector is the zero vector we return a point on the
|
// If the direction vector is the zero vector we return a point on the
|
||||||
// boundary of the sphere
|
// 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
|
}; // End of the ReactPhysics3D namespace
|
||||||
|
|
|
@ -210,12 +210,12 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
|
||||||
}
|
}
|
||||||
|
|
||||||
// Associate the edges of neighbouring triangle faces
|
// Associate the edges of neighbouring triangle faces
|
||||||
EdgeEPA(face0, 0).link(EdgeEPA(face1, 2));
|
link(EdgeEPA(face0, 0), EdgeEPA(face1, 2));
|
||||||
EdgeEPA(face0, 1).link(EdgeEPA(face3, 2));
|
link(EdgeEPA(face0, 1), EdgeEPA(face3, 2));
|
||||||
EdgeEPA(face0, 2).link(EdgeEPA(face2, 0));
|
link(EdgeEPA(face0, 2), EdgeEPA(face2, 0));
|
||||||
EdgeEPA(face1, 0).link(EdgeEPA(face2, 2));
|
link(EdgeEPA(face1, 0), EdgeEPA(face2, 2));
|
||||||
EdgeEPA(face1, 1).link(EdgeEPA(face3, 0));
|
link(EdgeEPA(face1, 1), EdgeEPA(face3, 0));
|
||||||
EdgeEPA(face2, 1).link(EdgeEPA(face3, 1));
|
link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
|
||||||
|
|
||||||
// Add the triangle faces in the candidate heap
|
// Add the triangle faces in the candidate heap
|
||||||
addFaceCandidate(face0, triangleHeap, nbTriangles, DBL_MAX);
|
addFaceCandidate(face0, triangleHeap, nbTriangles, DBL_MAX);
|
||||||
|
@ -252,18 +252,20 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
|
||||||
suppPointsA[3] = boundingVolume1->getSupportPoint(n, OBJECT_MARGIN);
|
suppPointsA[3] = boundingVolume1->getSupportPoint(n, OBJECT_MARGIN);
|
||||||
suppPointsB[3] = boundingVolume2->getSupportPoint(n.getOpposite(), OBJECT_MARGIN);
|
suppPointsB[3] = boundingVolume2->getSupportPoint(n.getOpposite(), OBJECT_MARGIN);
|
||||||
points[3] = suppPointsA[3] - suppPointsB[3];
|
points[3] = suppPointsA[3] - suppPointsB[3];
|
||||||
suppPointsA[4] = boundingVolume1->getSupportPoint(n.getOpposite(), OBJECT_MARGIN);
|
suppPointsA[4] = boundingVolume1->getSupportPoint(n.getOpposite(), OBJECT_MARGIN); // TODO : Uncomment this
|
||||||
suppPointsB[4] = boundingVolume2->getSupportPoint(n, OBJECT_MARGIN);
|
suppPointsB[4] = boundingVolume2->getSupportPoint(n, OBJECT_MARGIN); // TODO : Uncomment this
|
||||||
points[4] = suppPointsA[4] - suppPointsB[4];
|
points[4] = suppPointsA[4] - suppPointsB[4]; // TODO : Uncomment this
|
||||||
|
|
||||||
// Construct the triangle faces
|
// Construct the triangle faces
|
||||||
TriangleEPA* face0 = triangleStore.newTriangle(points, 0, 1, 3);
|
TriangleEPA* face0 = triangleStore.newTriangle(points, 0, 1, 3);
|
||||||
TriangleEPA* face1 = triangleStore.newTriangle(points, 1, 2, 3);
|
TriangleEPA* face1 = triangleStore.newTriangle(points, 1, 2, 3);
|
||||||
TriangleEPA* face2 = triangleStore.newTriangle(points, 2, 0, 3);
|
TriangleEPA* face2 = triangleStore.newTriangle(points, 2, 0, 3);
|
||||||
TriangleEPA* face3 = triangleStore.newTriangle(points, 0, 2, 4);
|
//TriangleEPA* face3 = triangleStore.newTriangle(points, 0, 1, 2); // TODO : Remove this
|
||||||
TriangleEPA* face4 = triangleStore.newTriangle(points, 2, 1, 4);
|
TriangleEPA* face3 = triangleStore.newTriangle(points, 0, 2, 4);// TODO : Uncomment this
|
||||||
TriangleEPA* face5 = triangleStore.newTriangle(points, 1, 0, 4);
|
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 the polytope hasn't been correctly constructed
|
||||||
if (!(face0 && face1 && face2 && face3 && face4 && face5 &&
|
if (!(face0 && face1 && face2 && face3 && face4 && face5 &&
|
||||||
face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 &&
|
face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0 &&
|
||||||
|
@ -272,26 +274,37 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
|
||||||
return false;
|
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
|
// Associate the edges of neighbouring faces
|
||||||
EdgeEPA(face0, 1).link(EdgeEPA(face1, 2));
|
link(EdgeEPA(face0, 1), EdgeEPA(face1, 2));
|
||||||
EdgeEPA(face1, 1).link(EdgeEPA(face2, 2));
|
link(EdgeEPA(face1, 1), EdgeEPA(face2, 2));
|
||||||
EdgeEPA(face2, 1).link(EdgeEPA(face0, 2));
|
link(EdgeEPA(face2, 1), EdgeEPA(face0, 2));
|
||||||
EdgeEPA(face0, 0).link(EdgeEPA(face5, 0));
|
link(EdgeEPA(face0, 0), EdgeEPA(face5, 0));
|
||||||
EdgeEPA(face1, 0).link(EdgeEPA(face4, 0));
|
link(EdgeEPA(face1, 0), EdgeEPA(face4, 0));
|
||||||
EdgeEPA(face2, 0).link(EdgeEPA(face3, 0));
|
link(EdgeEPA(face2, 0), EdgeEPA(face3, 0));
|
||||||
EdgeEPA(face3, 1).link(EdgeEPA(face4, 2));
|
link(EdgeEPA(face3, 1), EdgeEPA(face4, 2));
|
||||||
EdgeEPA(face4, 1).link(EdgeEPA(face5, 2));
|
link(EdgeEPA(face4, 1), EdgeEPA(face5, 2));
|
||||||
EdgeEPA(face5, 1).link(EdgeEPA(face3, 2));
|
link(EdgeEPA(face5, 1), EdgeEPA(face3, 2));
|
||||||
|
|
||||||
// Add the candidate faces in the heap
|
// Add the candidate faces in the heap
|
||||||
addFaceCandidate(face0, triangleHeap, nbTriangles, DBL_MAX);
|
addFaceCandidate(face0, triangleHeap, nbTriangles, DBL_MAX);
|
||||||
addFaceCandidate(face1, triangleHeap, nbTriangles, DBL_MAX);
|
addFaceCandidate(face1, triangleHeap, nbTriangles, DBL_MAX);
|
||||||
addFaceCandidate(face2, triangleHeap, nbTriangles, DBL_MAX);
|
addFaceCandidate(face2, triangleHeap, nbTriangles, DBL_MAX);
|
||||||
addFaceCandidate(face3, triangleHeap, nbTriangles, DBL_MAX);
|
addFaceCandidate(face3, triangleHeap, nbTriangles, DBL_MAX);
|
||||||
addFaceCandidate(face4, triangleHeap, nbTriangles, DBL_MAX);
|
addFaceCandidate(face4, triangleHeap, nbTriangles, DBL_MAX); // TODO : Uncomment this
|
||||||
addFaceCandidate(face5, triangleHeap, nbTriangles, DBL_MAX);
|
addFaceCandidate(face5, triangleHeap, nbTriangles, DBL_MAX); // TODO : Uncomment this
|
||||||
|
|
||||||
nbVertices = 5;
|
nbVertices = 5; // TODO : Uncomment this
|
||||||
|
//nbVertices = 4; // TODO : Remove this
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
@ -338,6 +351,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
|
||||||
}
|
}
|
||||||
|
|
||||||
// Compute the error
|
// Compute the error
|
||||||
|
double distSquare = triangle->getDistSquare(); // TODO : REMOVE THIS
|
||||||
double error = wDotv - triangle->getDistSquare();
|
double error = wDotv - triangle->getDistSquare();
|
||||||
if (error <= std::max(tolerance, REL_ERROR_SQUARE * wDotv) ||
|
if (error <= std::max(tolerance, REL_ERROR_SQUARE * wDotv) ||
|
||||||
points[indexNewVertex] == points[(*triangle)[0]] ||
|
points[indexNewVertex] == points[(*triangle)[0]] ||
|
||||||
|
@ -368,7 +382,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
|
||||||
} while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
|
} while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
|
||||||
|
|
||||||
// Compute the contact info
|
// 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 pA = triangle->computeClosestPointOfObject(suppPointsA);
|
||||||
Vector3D pB = triangle->computeClosestPointOfObject(suppPointsB);
|
Vector3D pB = triangle->computeClosestPointOfObject(suppPointsB);
|
||||||
Vector3D diff = pB - pA;
|
Vector3D diff = pB - pA;
|
||||||
|
|
|
@ -31,6 +31,7 @@
|
||||||
// We want to use the ReactPhysics3D namespace
|
// We want to use the ReactPhysics3D namespace
|
||||||
using namespace reactphysics3d;
|
using namespace reactphysics3d;
|
||||||
|
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
EdgeEPA::EdgeEPA() {
|
EdgeEPA::EdgeEPA() {
|
||||||
|
|
||||||
|
@ -57,30 +58,6 @@ uint EdgeEPA::getTarget() const {
|
||||||
return (*ownerTriangle)[indexOfNextCounterClockwiseEdge(index)];
|
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
|
// Compute the silhouette
|
||||||
bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesStore& triangleStore) {
|
bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesStore& triangleStore) {
|
||||||
// If the edge has not already been visited
|
// 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 the triangle has been created
|
||||||
if (triangle) {
|
if (triangle) {
|
||||||
EdgeEPA(triangle, 1).halfLink(*this);
|
halfLink(EdgeEPA(triangle, 1), *this);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -111,7 +88,7 @@ bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesS
|
||||||
|
|
||||||
// If the triangle has been created
|
// If the triangle has been created
|
||||||
if (triangle) {
|
if (triangle) {
|
||||||
EdgeEPA(triangle, 1).halfLink(*this);
|
halfLink(EdgeEPA(triangle, 1), *this);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -125,7 +102,7 @@ bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesS
|
||||||
TriangleEPA* triangle = triangleStore.newTriangle(vertices, index, getTarget(), getSource());
|
TriangleEPA* triangle = triangleStore.newTriangle(vertices, index, getTarget(), getSource());
|
||||||
|
|
||||||
if (triangle) {
|
if (triangle) {
|
||||||
EdgeEPA(triangle, 1).halfLink(*this);
|
halfLink(EdgeEPA(triangle, 1), *this);
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -56,13 +56,10 @@ class EdgeEPA {
|
||||||
int getIndex() const; // Return the index of the edge in the 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 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 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,
|
bool computeSilhouette(const Vector3D* vertices, uint index,
|
||||||
TrianglesStore& triangleStore); // Compute the recursive silhouette algorithm
|
TrianglesStore& triangleStore); // Compute the recursive silhouette algorithm
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
// Return the pointer to the owner triangle
|
// Return the pointer to the owner triangle
|
||||||
inline TriangleEPA* EdgeEPA::getOwnerTriangle() const {
|
inline TriangleEPA* EdgeEPA::getOwnerTriangle() const {
|
||||||
return ownerTriangle;
|
return ownerTriangle;
|
||||||
|
|
|
@ -82,6 +82,28 @@ bool TriangleEPA::computeClosestPoint(const Vector3D* vertices) {
|
||||||
return false;
|
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
|
// Compute recursive silhouette algorithm for that triangle
|
||||||
bool TriangleEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesStore& triangleStore) {
|
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;
|
int i,j;
|
||||||
for (i=first, j=triangleStore.getNbTriangles()-1; i != triangleStore.getNbTriangles(); j = i++) {
|
for (i=first, j=triangleStore.getNbTriangles()-1; i != triangleStore.getNbTriangles(); j = i++) {
|
||||||
TriangleEPA* triangle = &triangleStore[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;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -34,6 +34,11 @@
|
||||||
// ReactPhysics3D namespace
|
// ReactPhysics3D namespace
|
||||||
namespace reactphysics3d {
|
namespace reactphysics3d {
|
||||||
|
|
||||||
|
// Prototypes
|
||||||
|
bool link(const EdgeEPA& edge0, const EdgeEPA& edge1);
|
||||||
|
void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1);
|
||||||
|
|
||||||
|
|
||||||
/* -------------------------------------------------------------------
|
/* -------------------------------------------------------------------
|
||||||
Class TriangleEPA :
|
Class TriangleEPA :
|
||||||
This class represents a triangle face of the current polytope in the EPA
|
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
|
TrianglesStore& triangleStore); // Compute recursive silhouette algorithm for that triangle
|
||||||
|
|
||||||
uint operator[](int i) const; // Access operator
|
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
|
// 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
|
// Return true if the closest point on affine hull is inside the triangle
|
||||||
inline bool TriangleEPA::isClosestPointInternalToTriangle() const {
|
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
|
// Return true if the triangle is visible from a given vertex
|
||||||
|
|
|
@ -90,8 +90,7 @@ inline TriangleEPA* TrianglesStore::newTriangle(const Vector3D* vertices, uint v
|
||||||
|
|
||||||
// If we have not reach the maximum number of triangles
|
// If we have not reach the maximum number of triangles
|
||||||
if (nbTriangles != MAX_TRIANGLES) {
|
if (nbTriangles != MAX_TRIANGLES) {
|
||||||
newTriangle = &triangles[nbTriangles];
|
newTriangle = &triangles[nbTriangles++];
|
||||||
nbTriangles++;
|
|
||||||
new (newTriangle) TriangleEPA(v0, v1, v2);
|
new (newTriangle) TriangleEPA(v0, v1, v2);
|
||||||
if (!newTriangle->computeClosestPoint(vertices)) {
|
if (!newTriangle->computeClosestPoint(vertices)) {
|
||||||
nbTriangles--;
|
nbTriangles--;
|
||||||
|
|
Loading…
Reference in New Issue
Block a user