From 9fae1b4e35830dcec7cbc97a51e7a71b7309ed9e Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 20 Jun 2016 08:40:26 +0200 Subject: [PATCH 1/7] Add missing virtual destructor --- src/collision/CollisionDetection.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 4bc4f319..7e81a25f 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -62,6 +62,9 @@ class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback { } + // Destructor + virtual ~TestCollisionBetweenShapesCallback() { } + // Called by a narrow-phase collision algorithm when a new contact has been found virtual void notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo); From fd224ebabae357f5578e435198b7ec253efd52a9 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 20 Jun 2016 08:41:22 +0200 Subject: [PATCH 2/7] Add VoronoiSimplex class for GJK algorithm --- .../narrowphase/GJK/VoronoiSimplex.cpp | 618 ++++++++++++++++++ .../narrowphase/GJK/VoronoiSimplex.h | 201 ++++++ 2 files changed, 819 insertions(+) create mode 100644 src/collision/narrowphase/GJK/VoronoiSimplex.cpp create mode 100644 src/collision/narrowphase/GJK/VoronoiSimplex.h diff --git a/src/collision/narrowphase/GJK/VoronoiSimplex.cpp b/src/collision/narrowphase/GJK/VoronoiSimplex.cpp new file mode 100644 index 00000000..cf7acd7a --- /dev/null +++ b/src/collision/narrowphase/GJK/VoronoiSimplex.cpp @@ -0,0 +1,618 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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 "VoronoiSimplex.h" +#include + +// We want to use the ReactPhysics3D namespace +using namespace reactphysics3d; + +decimal VoronoiSimplex::epsilon = decimal(0.0001); + +// Constructor +VoronoiSimplex::VoronoiSimplex() : mNbPoints(0), mRecomputeClosestPoint(false), mIsClosestPointValid(false) { + +} + +// Destructor +VoronoiSimplex::~VoronoiSimplex() { + +} + +// Add a new support point of (A-B) into the simplex +/// suppPointA : support point of object A in a direction -v +/// suppPointB : support point of object B in a direction v +/// point : support point of object (A-B) => point = suppPointA - suppPointB +void VoronoiSimplex::addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB) { + assert(!isFull()); + + mPoints[mNbPoints] = point; + mSuppPointsA[mNbPoints] = suppPointA; + mSuppPointsB[mNbPoints] = suppPointB; + + mNbPoints++; + mRecomputeClosestPoint = true; +} + +// Remove a point from the simplex +void VoronoiSimplex::removePoint(int index) { + assert(mNbPoints > 0); + mNbPoints--; + mPoints[index] = mPoints[mNbPoints]; + mSuppPointsA[index] = mSuppPointsA[mNbPoints]; + mSuppPointsB[index] = mSuppPointsA[mNbPoints]; +} + +// Reduce the simplex (only keep vertices that participate to the point closest to the origin) +/// bitsUsedPoints is seen as a sequence of bits representing whether the four points of +/// the simplex are used or not to represent the current closest point to the origin. +/// - The most right bit is set to one if the first point is used +/// - The second most right bit is set to one if the second point is used +/// - The third most right bit is set to one if the third point is used +/// - The fourth most right bit is set to one if the fourth point is used +void VoronoiSimplex::reduceSimplex(int bitsUsedPoints) { + + if ((mNbPoints >= 4) && (bitsUsedPoints & 8) == 0) + removePoint(3); + + if ((mNbPoints >= 3) && (bitsUsedPoints & 4) == 0) + removePoint(2); + + if ((mNbPoints >= 2) && (bitsUsedPoints & 2) == 0) + removePoint(1); + + if ((mNbPoints >= 1) && (bitsUsedPoints & 1) == 0) + removePoint(0); +} + +// Return true if the point is in the simplex +bool VoronoiSimplex::isPointInSimplex(const Vector3& point) const { + + // For each four possible points in the simplex + for (int i=0; i= 0 && mNbPoints <= 4); + + switch(mNbPoints) { + case 0: return false; + case 1: return false; + + // Two points are independent if there distance is larger than zero + case 2: return (mPoints[1] - mPoints[0]).lengthSquare() <= epsilon; + + // Three points are independent if the triangle area is larger than zero + case 3: return (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]).lengthSquare() <= epsilon; + + // Four points are independent if the tetrahedron volume is larger than zero + case 4: return std::abs((mPoints[1] - mPoints[0]).dot((mPoints[2] - mPoints[0]).cross(mPoints[3] - mPoints[0]))) <= epsilon; + } + + return false; +} + +// Compute the closest points "pA" and "pB" of object A and B. +/// The points are computed as follows : +/// pA = sum(lambda_i * a_i) where "a_i" are the support points of object A +/// pB = sum(lambda_i * b_i) where "b_i" are the support points of object B +/// with lambda_i = deltaX_i / deltaX +void VoronoiSimplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) { + + // Recompute the closest point (if necessary) + recomputeClosestPoint(); + + pA = mSuppPointsA; + pB = mSuppPointsB; +} + +// Recompute the closest point if the simplex has been modified +/// This method computes the point "v" of simplex that is closest to the origin. +/// The method returns true if a closest point has been found. +bool VoronoiSimplex::recomputeClosestPoint() { + + assert(mNbPoints <= 4); + + // If we need to recompute the closest point + if (mRecomputeClosestPoint) { + + mRecomputeClosestPoint = false; + + switch(mNbPoints) { + + case 0: + + // Cannot compute closest point when the simplex is empty + mIsClosestPointValid = false; + break; + + case 1: + + { + // There is a single point in the simplex, therefore, this point is + // the one closest to the origin + mClosestPoint = mPoints[0]; + mClosestSuppPointA = mSuppPointsA[0]; + mClosestSuppPointB = mSuppPointsB[0]; + setBarycentricCoords(1, 0, 0, 0); + mIsClosestPointValid = checkClosestPointValid(); + } + break; + + case 2: + + { + int bitsUsedPoints = 0; + + // The simplex is a line AB (where A=mPoints[0] and B=mPoints[1]. + // We need to find the point of that line closest to the origin + Vector3 AP = -mPoints[0]; + Vector3 AB = mPoints[1] - mPoints[0]; + decimal APDotAB = AP.dot(AB); + decimal t; + + // If the closest point is on the side of A in the direction of B + if (APDotAB > decimal(0.0)) { + decimal lengthABSquare = AB.lengthSquare(); + + // If the closest point is on the segment AB + if (APDotAB < lengthABSquare) { + t = APDotAB / lengthABSquare; + + bitsUsedPoints = 3; // 0011 (both A and B are used) + + } + else { // If the origin is on the side of B that is not in the direction of A + // Therefore, the closest point is B + t = decimal(1.0); + + bitsUsedPoints = 2; // 0010 (only B is used) + } + } + else { // If the origin is on the side of A that is not in the direction of B + // Therefore, the closest point of the line is A + t = decimal(0.0); + + bitsUsedPoints = 1; // 0001 (only A is used) + } + + // Compute the closest point + mClosestSuppPointA = mSuppPointsA[0] + t * (mSuppPointsA[1] - mSuppPointsA[0]); + mClosestSuppPointB = mSuppPointsB[0] + t * (mSuppPointsB[1] - mSuppPointsB[0]); + mClosestPoint = mClosestSuppPointA - mClosestSuppPointB; + setBarycentricCoords(decimal(1.0) - t, t, 0, 0); + mIsClosestPointValid = checkClosestPointValid(); + + // Reduce the simplex (remove vertices that are not participating to + // the closest point + reduceSimplex(bitsUsedPoints); + } + break; + + case 3: + { + // The simplex is a triangle. We need to find the point of that + // triangle that is closest to the origin + + int bitsUsedVertices = 0; + Vector3 baryCoords; + + // Compute the point of the triangle closest to the origin + computeClosestPointOnTriangle(mPoints[0], mPoints[1], mPoints[2], bitsUsedVertices, baryCoords); + mClosestSuppPointA = baryCoords[0] * mSuppPointsA[0] + baryCoords[1] * mSuppPointsA[1] + + baryCoords[2] * mSuppPointsA[2]; + mClosestSuppPointB = baryCoords[0] * mSuppPointsB[0] + baryCoords[1] * mSuppPointsB[1] + + baryCoords[2] * mSuppPointsB[2]; + mClosestPoint = mClosestSuppPointA - mClosestSuppPointB; + + setBarycentricCoords(baryCoords.x, baryCoords.y, baryCoords.z, 0.0); + mIsClosestPointValid = checkClosestPointValid(); + + // Reduce the simplex (remove vertices that are not participating to + // the closest point + reduceSimplex(bitsUsedVertices); + } + break; + + case 4: + + { + // The simplex is a tetrahedron. We need to find the point of that + // tetrahedron that is closest to the origin + + int bitsUsedVertices = 0; + Vector2 baryCoordsAB; + Vector2 baryCoordsCD; + + // Compute the point closest to the origin on the tetrahedron + bool isOutside = computeClosestPointOnTetrahedron(mPoints[0], mPoints[1], mPoints[2], mPoints[3], + bitsUsedVertices, baryCoordsAB, baryCoordsCD); + + // If the origin is outside the tetrahedron + if (isOutside) { + + // Compute the point of the tetrahedron closest to the origin + mClosestSuppPointA = baryCoordsAB.x * mSuppPointsA[0] + baryCoordsAB.y * mSuppPointsA[1] + + baryCoordsCD.x * mSuppPointsA[2] + baryCoordsCD.y * mSuppPointsA[3]; + mClosestSuppPointB = baryCoordsAB.x * mSuppPointsB[0] + baryCoordsAB.y * mSuppPointsB[1] + + baryCoordsCD.x * mSuppPointsB[2] + baryCoordsCD.y * mSuppPointsB[3]; + mClosestPoint = mClosestSuppPointA - mClosestSuppPointB; + + setBarycentricCoords(baryCoordsAB.x, baryCoordsAB.y, baryCoordsCD.x, baryCoordsCD.y); + + // Reduce the simplex (remove vertices that are not participating to + // the closest point + reduceSimplex(bitsUsedVertices); + } + else { + + // The origin is inside the tetrahedron, therefore, the closest point + // is the origin + + setBarycentricCoords(0.0, 0.0, 0.0, 0.0); + + mClosestSuppPointA.setToZero(); + mClosestSuppPointB.setToZero(); + mClosestPoint.setToZero(); + + mIsClosestPointValid = true; + + break; + } + + mIsClosestPointValid = checkClosestPointValid(); + + } + break; + } + } + + return mIsClosestPointValid; +} + +// Compute point on a triangle that is closest to the origin +/// This implementation is based on the one in the book +/// "Real-Time Collision Detection" by Christer Ericson. +void VoronoiSimplex::computeClosestPointOnTriangle(const Vector3& a, const Vector3& b, + const Vector3& c, int& bitsUsedVertices, Vector3& baryCoordsABC) const { + + // Check if the origin is in the Voronoi region of vertex A + Vector3 ab = b - a; + Vector3 ac = c - a; + Vector3 ap = -a; + decimal d1 = ab.dot(ap); + decimal d2 = ac.dot(ap); + if (d1 <= decimal(0.0) && d2 <= decimal(0.0)) { + + // The origin is in the Voronoi region of vertex A + + // Set the barycentric coords of the closest point on the triangle + baryCoordsABC.setAllValues(1.0, 0, 0); + + bitsUsedVertices = 1; // 0001 (only A is used) + return; + } + + // Check if the origin is in the Voronoi region of vertex B + Vector3 bp = -b; + decimal d3 = ab.dot(bp); + decimal d4 = ac.dot(bp); + if (d3 >= decimal(0.0) && d4 <= d3) { + + // The origin is in the Voronoi region of vertex B + + // Set the barycentric coords of the closest point on the triangle + baryCoordsABC.setAllValues(0.0, 1.0, 0); + + bitsUsedVertices = 2; // 0010 (only B is used) + return; + } + + // Check if the origin is in the Voronoi region of edge AB + decimal vc = d1 * d4 - d3 * d2; + if (vc <= decimal(0.0) && d1 >= decimal(0.0) && d3 <= decimal(0.0)) { + + // The origin is in the Voronoi region of edge AB + // We return the projection of the origin on the edge AB + decimal v = d1 / (d1 - d3); + + // Set the barycentric coords of the closest point on the triangle + baryCoordsABC.setAllValues(decimal(1.0) - v, v, 0); + + bitsUsedVertices = 3; // 0011 (A and B are used) + return; + } + + // Check if the origin is in the Voronoi region of vertex C + Vector3 cp = -c; + decimal d5 = ab.dot(cp); + decimal d6 = ac.dot(cp); + if (d6 >= decimal(0.0) && d5 <= d6) { + + // The origin is in the Voronoi region of vertex C + + // Set the barycentric coords of the closest point on the triangle + baryCoordsABC.setAllValues(0.0, 0.0, 1.0); + + bitsUsedVertices = 4; // 0100 (only C is used) + return; + } + + // Check if the origin is in the Voronoi region of edge AC + decimal vb = d5 * d2 - d1 * d6; + if (vb <= decimal(0.0) && d2 >= decimal(0.0) && d6 <= decimal(0.0)) { + + // The origin is in the Voronoi region of edge AC + // We return the projection of the origin on the edge AC + decimal w = d2 / (d2 - d6); + + // Set the barycentric coords of the closest point on the triangle + baryCoordsABC.setAllValues(decimal(1.0) - w, 0, w); + + bitsUsedVertices = 5; // 0101 (A and C are used) + return; + } + + // Check if the origin is in the Voronoi region of edge BC + decimal va = d3 * d6 - d5 * d4; + if (va <= decimal(0.0) && (d4 - d3) >= decimal(0.0) && (d5 - d6) >= decimal(0.0)) { + + // The origin is in the Voronoi region of edge BC + // We return the projection of the origin on the edge BC + decimal w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); + + // Set the barycentric coords of the closest point on the triangle + baryCoordsABC.setAllValues(0.0, decimal(1.0) - w, w); + + bitsUsedVertices = 6; // 0110 (B and C are used) + return; + } + + // The origin is in the Voronoi region of the face ABC + decimal denom = decimal(1.0) / (va + vb + vc); + decimal v = vb * denom; + decimal w = vc * denom; + + // Set the barycentric coords of the closest point on the triangle + baryCoordsABC.setAllValues(1 - v - w, v, w); + + bitsUsedVertices = 7; // 0111 (A, B and C are used) + return; +} + +// Compute point of a tetrahedron that is closest to the origin +/// This implementation is based on the one in the book +/// "Real-Time Collision Detection" by Christer Ericson. +/// This method returns true if the origin is outside the tetrahedron. +bool VoronoiSimplex::computeClosestPointOnTetrahedron(const Vector3& a, const Vector3& b, const Vector3& c, + const Vector3& d, int& bitsUsedPoints, Vector2& baryCoordsAB, + Vector2& baryCoordsCD) const { + + // Start as if the origin was inside the tetrahedron + bitsUsedPoints = 15; // 1111 (A, B, C and D are used) + baryCoordsAB.setToZero(); + baryCoordsCD.setToZero(); + + // Check if the origin is outside each tetrahedron face + int isOriginOutsideFaceABC = testOriginOutsideOfPlane(a, b, c, d); + int isOriginOutsideFaceACD = testOriginOutsideOfPlane(a, c, d, b); + int isOriginOutsideFaceADB = testOriginOutsideOfPlane(a, d, b, c); + int isOriginOutsideFaceBDC = testOriginOutsideOfPlane(b, d, c, a); + + if (isOriginOutsideFaceABC == 0 && isOriginOutsideFaceACD == 0 && + isOriginOutsideFaceADB == 0 && isOriginOutsideFaceBDC == 0) { + + // The origin is inside the tetrahedron + return true; + } + + // We know that the origin is outside the tetrahedron, we now need to find + // which of the four triangle faces is closest to it. + + decimal closestSquareDistance = DECIMAL_LARGEST; + int tempUsedVertices; + Vector3 triangleBaryCoords; + + // If the origin is outside face ABC + if (isOriginOutsideFaceABC) { + + // Compute the closest point on this triangle face + computeClosestPointOnTriangle(a, b, c, tempUsedVertices, triangleBaryCoords); + Vector3 closestPoint = triangleBaryCoords[0] * a + triangleBaryCoords[1] * b + + triangleBaryCoords[2] * c; + decimal squareDist = closestPoint.lengthSquare(); + + // If the point on that face is the closest to the origin so far + if (squareDist < closestSquareDistance) { + + // Use it as the current closest point + closestSquareDistance = squareDist; + baryCoordsAB.setAllValues(triangleBaryCoords[0], triangleBaryCoords[1]); + baryCoordsCD.setAllValues(triangleBaryCoords[2], 0.0); + bitsUsedPoints = tempUsedVertices; + } + } + + // If the origin is outside face ACD + if (isOriginOutsideFaceACD) { + + // Compute the closest point on this triangle face + computeClosestPointOnTriangle(a, c, d, tempUsedVertices, triangleBaryCoords); + Vector3 closestPoint = triangleBaryCoords[0] * a + triangleBaryCoords[1] * c + + triangleBaryCoords[2] * d; + decimal squareDist = closestPoint.lengthSquare(); + + // If the point on that face is the closest to the origin so far + if (squareDist < closestSquareDistance) { + + // Use it as the current closest point + closestSquareDistance = squareDist; + baryCoordsAB.setAllValues(triangleBaryCoords[0], 0.0); + baryCoordsCD.setAllValues(triangleBaryCoords[1], triangleBaryCoords[2]); + bitsUsedPoints = mapTriangleUsedVerticesToTetrahedron(tempUsedVertices, 0, 2, 3); + } + } + + // If the origin is outside face + if (isOriginOutsideFaceADB) { + + // Compute the closest point on this triangle face + computeClosestPointOnTriangle(a, d, b, tempUsedVertices, triangleBaryCoords); + Vector3 closestPoint = triangleBaryCoords[0] * a + triangleBaryCoords[1] * d + + triangleBaryCoords[2] * b; + decimal squareDist = closestPoint.lengthSquare(); + + // If the point on that face is the closest to the origin so far + if (squareDist < closestSquareDistance) { + + // Use it as the current closest point + closestSquareDistance = squareDist; + baryCoordsAB.setAllValues(triangleBaryCoords[0], triangleBaryCoords[2]); + baryCoordsCD.setAllValues(0.0, triangleBaryCoords[1]); + bitsUsedPoints = mapTriangleUsedVerticesToTetrahedron(tempUsedVertices, 0, 3, 1); + } + } + + // If the origin is outside face + if (isOriginOutsideFaceBDC) { + + // Compute the closest point on this triangle face + computeClosestPointOnTriangle(b, d, c, tempUsedVertices, triangleBaryCoords); + Vector3 closestPoint = triangleBaryCoords[0] * b + triangleBaryCoords[1] * d + + triangleBaryCoords[2] * c; + decimal squareDist = closestPoint.lengthSquare(); + + // If the point on that face is the closest to the origin so far + if (squareDist < closestSquareDistance) { + + // Use it as the current closest point + closestSquareDistance = squareDist; + baryCoordsAB.setAllValues(0.0, triangleBaryCoords[0]); + baryCoordsCD.setAllValues(triangleBaryCoords[2], triangleBaryCoords[1]); + bitsUsedPoints = mapTriangleUsedVerticesToTetrahedron(tempUsedVertices, 1, 3, 2); + } + } + + return true; +} + +// 0111 (1,2,3) => 0111 +// 0011 (2,1,3) => +int VoronoiSimplex::mapTriangleUsedVerticesToTetrahedron(int triangleUsedVertices, int first, int second, int third) const { + + assert(triangleUsedVertices <= 7); + int tetrahedronUsedVertices = (((1 & triangleUsedVertices) != 0) << first) | + (((2 & triangleUsedVertices) != 0) << second) | + (((4 & triangleUsedVertices) != 0) << third); + + assert(tetrahedronUsedVertices <= 14); + return tetrahedronUsedVertices; +} + +// Test if a point is outside of the plane given by the points of the triangle (a, b, c) +/// This method returns 1 if the point d and the origin are on opposite sides of +/// the triangle face (a, b, c). It returns 0 if they are on the same side. +/// This implementation is based on the one in the book +/// "Real-Time Collision Detection" by Christer Ericson. +int VoronoiSimplex::testOriginOutsideOfPlane(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d) const { + + // Normal of (a,b,c) triangle + const Vector3 n = (b-a).cross(c-a); + + decimal signp = (-a).dot(n); + decimal signd = (d-a).dot(n); + + // This assert is raised if the tetrahedron is degenerate (all points on the same plane) + // This should never happen because after a point is added into the simplex, the user + // of this class must check that the simplex is not affinely dependent using the + // isAffinelyDependent() method + assert(signd * signd > epsilon * epsilon); + + return signp * signd < decimal(0.0); +} + +// Backup the closest point +void VoronoiSimplex::backupClosestPointInSimplex(Vector3& v) { + decimal minDistSquare = DECIMAL_LARGEST; + Bits bit; + + for (bit = mAllBits; bit != 0x0; bit--) { + if (isSubset(bit, mAllBits) && isProperSubset(bit)) { + Vector3 u = computeClosestPointForSubset(bit); + decimal distSquare = u.dot(u); + if (distSquare < minDistSquare) { + minDistSquare = distSquare; + mBitsCurrentSimplex = bit; + v = u; + } + } + } +} + +// Return the maximum squared length of a point +decimal VoronoiSimplex::getMaxLengthSquareOfAPoint() const { + + decimal maxLengthSquare = decimal(0.0); + + for (int i=0; i maxLengthSquare) { + maxLengthSquare = lengthSquare; + } + } + + // Return the maximum squared length + return maxLengthSquare; +} diff --git a/src/collision/narrowphase/GJK/VoronoiSimplex.h b/src/collision/narrowphase/GJK/VoronoiSimplex.h new file mode 100644 index 00000000..9d1ba580 --- /dev/null +++ b/src/collision/narrowphase/GJK/VoronoiSimplex.h @@ -0,0 +1,201 @@ +/******************************************************************************** +* ReactPhysics3D physics library, http://www.reactphysics3d.com * +* Copyright (c) 2010-2016 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. * +* * +********************************************************************************/ + +#ifndef REACTPHYSICS3D_VORONOI_SIMPLEX_H +#define REACTPHYSICS3D_VORONOI_SIMPLEX_H + +// Libraries +#include "mathematics/mathematics.h" +#include + +/// ReactPhysics3D namespace +namespace reactphysics3d { + +// Type definitions +typedef unsigned int Bits; + +// Class VoronoiSimplex +/** + * This class represents a simplex which is a set of 3D points. This + * class is used in the GJK algorithm. This implementation is based in the book + * "Real-Time Collision Detection" by Christer Ericson. This simple is used to + * replace theJohnson's algorithm for computing the point of a simplex that + * is closest to the origin and also the smallest simplex needed to represent + * that closest point. + */ +class VoronoiSimplex { + + private: + + // -------------------- Attributes -------------------- // + + /// Current points + Vector3 mPoints[4]; + + /// Number of vertices in the simplex + int mNbPoints; + + /// Barycentric coordinates of the closest point using simplex vertices + decimal mBarycentricCoords[4]; + + /// pointsLengthSquare[i] = (points[i].length)^2 + decimal mPointsLengthSquare[4]; + + /// Support points of object A in local coordinates + Vector3 mSuppPointsA[4]; + + /// Support points of object B in local coordinates + Vector3 mSuppPointsB[4]; + + /// True if the closest point has to be recomputed (because the simplex has changed) + bool mRecomputeClosestPoint; + + /// Current point that is closest to the origin + Vector3 mClosestPoint; + + /// Current closest point on object A + Vector3 mClosestSuppPointA; + + /// Current closest point on object B + Vector3 mClosestSuppPointB; + + /// True if the last computed closest point is valid + bool mIsClosestPointValid; + + /// Epsilon value + static decimal epsilon; + + // -------------------- Methods -------------------- // + + /// Private copy-constructor + VoronoiSimplex(const VoronoiSimplex& simplex); + + /// Private assignment operator + VoronoiSimplex& operator=(const VoronoiSimplex& simplex); + + /// Set the barycentric coordinates of the closest point + void setBarycentricCoords(decimal a, decimal b, decimal c, decimal d); + + /// Recompute the closest point if the simplex has been modified + bool recomputeClosestPoint(); + + /// Return true if the + bool checkClosestPointValid() const; + + /// Compute point of a triangle that is closest to the origin + void computeClosestPointOnTriangle(const Vector3& a, const Vector3& b, + const Vector3& c, int& bitsUsedPoints, Vector3& baryCoordsABC) const; + + /// Compute point of a tetrahedron that is closest to the origin + bool computeClosestPointOnTetrahedron(const Vector3& a, const Vector3& b, + const Vector3& c, const Vector3& d, + int& bitsUsedPoints, Vector2& baryCoordsAB, Vector2& baryCoordsCD) const; + + /// Test if a point is outside of the plane given by the points of the triangle (a, b, c) + int testOriginOutsideOfPlane(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d) const; + + /// Remap the used vertices bits of a triangle to the corresponding used vertices of a tetrahedron + int mapTriangleUsedVerticesToTetrahedron(int triangleUsedVertices, int first, int second, int third) const; + + public: + + // -------------------- Methods -------------------- // + + /// Constructor + VoronoiSimplex(); + + /// Destructor + ~VoronoiSimplex(); + + /// Return true if the simplex contains 4 points + bool isFull() const; + + /// Return true if the simplex is empty + bool isEmpty() const; + + /// Return the points of the simplex + int getSimplex(Vector3* mSuppPointsA, Vector3* mSuppPointsB, Vector3* mPoints) const; + + /// Return the maximum squared length of a point + decimal getMaxLengthSquareOfAPoint() const; + + /// Add a new support point of (A-B) into the simplex + void addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB); + + /// Remove a point from the simplex + void removePoint(int index); + + /// Reduce the simplex (only keep vertices that participate to the point closest to the origin) + void reduceSimplex(int bitsUsedPoints); + + /// Return true if the point is in the simplex + bool isPointInSimplex(const Vector3& point) const; + + /// Return true if the set is affinely dependent + bool isAffinelyDependent() const; + + /// Backup the closest point + void backupClosestPointInSimplex(Vector3& point); + + /// Compute the closest points "pA" and "pB" of object A and B. + void computeClosestPointsOfAandB(Vector3& pA, Vector3& pB); + + /// Compute the closest point to the origin of the current simplex. + bool computeClosestPoint(Vector3& v); +}; + +// Return true if the simplex contains 4 points +inline bool VoronoiSimplex::isFull() const { + return mNbPoints == 4; +} + +// Return true if the simple is empty +inline bool VoronoiSimplex::isEmpty() const { + return mNbPoints == 0; +} + +// Set the barycentric coordinates of the closest point +inline void VoronoiSimplex::setBarycentricCoords(decimal a, decimal b, decimal c, decimal d) { + mBarycentricCoords[0] = a; + mBarycentricCoords[1] = b; + mBarycentricCoords[2] = c; + mBarycentricCoords[3] = d; +} + +// Compute the closest point "v" to the origin of the current simplex. +inline bool VoronoiSimplex::computeClosestPoint(Vector3& v) { + + bool isValid = recomputeClosestPoint(); + v = mClosestPoint; + return isValid; +} + +// Return true if the +inline bool VoronoiSimplex::checkClosestPointValid() const { + return mBarycentricCoords[0] >= decimal(0.0) && mBarycentricCoords[1] >= decimal(0.0) && + mBarycentricCoords[2] >= decimal(0.0) && mBarycentricCoords[3] >= decimal(0.0); +} + +#endif From ccd33c2502350af867dfb8659d2f004dd8a66e44 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 27 Jun 2016 18:50:12 +0200 Subject: [PATCH 3/7] Fix issue in VoronoiSimplex --- .../narrowphase/GJK/VoronoiSimplex.cpp | 136 ++++++++++-------- .../narrowphase/GJK/VoronoiSimplex.h | 9 +- 2 files changed, 83 insertions(+), 62 deletions(-) diff --git a/src/collision/narrowphase/GJK/VoronoiSimplex.cpp b/src/collision/narrowphase/GJK/VoronoiSimplex.cpp index cf7acd7a..a30f52ff 100644 --- a/src/collision/narrowphase/GJK/VoronoiSimplex.cpp +++ b/src/collision/narrowphase/GJK/VoronoiSimplex.cpp @@ -63,7 +63,7 @@ void VoronoiSimplex::removePoint(int index) { mNbPoints--; mPoints[index] = mPoints[mNbPoints]; mSuppPointsA[index] = mSuppPointsA[mNbPoints]; - mSuppPointsB[index] = mSuppPointsA[mNbPoints]; + mSuppPointsB[index] = mSuppPointsB[mNbPoints]; } // Reduce the simplex (only keep vertices that participate to the point closest to the origin) @@ -138,6 +138,7 @@ bool VoronoiSimplex::isAffinelyDependent() const { case 3: return (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]).lengthSquare() <= epsilon; // Four points are independent if the tetrahedron volume is larger than zero + // Test in three different ways (for more robustness) case 4: return std::abs((mPoints[1] - mPoints[0]).dot((mPoints[2] - mPoints[0]).cross(mPoints[3] - mPoints[0]))) <= epsilon; } @@ -152,10 +153,10 @@ bool VoronoiSimplex::isAffinelyDependent() const { void VoronoiSimplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) { // Recompute the closest point (if necessary) - recomputeClosestPoint(); + //recomputeClosestPoint(); - pA = mSuppPointsA; - pB = mSuppPointsB; + pA = mClosestSuppPointA; + pB = mClosestSuppPointB; } // Recompute the closest point if the simplex has been modified @@ -195,38 +196,11 @@ bool VoronoiSimplex::recomputeClosestPoint() { { int bitsUsedPoints = 0; + float t; // The simplex is a line AB (where A=mPoints[0] and B=mPoints[1]. // We need to find the point of that line closest to the origin - Vector3 AP = -mPoints[0]; - Vector3 AB = mPoints[1] - mPoints[0]; - decimal APDotAB = AP.dot(AB); - decimal t; - - // If the closest point is on the side of A in the direction of B - if (APDotAB > decimal(0.0)) { - decimal lengthABSquare = AB.lengthSquare(); - - // If the closest point is on the segment AB - if (APDotAB < lengthABSquare) { - t = APDotAB / lengthABSquare; - - bitsUsedPoints = 3; // 0011 (both A and B are used) - - } - else { // If the origin is on the side of B that is not in the direction of A - // Therefore, the closest point is B - t = decimal(1.0); - - bitsUsedPoints = 2; // 0010 (only B is used) - } - } - else { // If the origin is on the side of A that is not in the direction of B - // Therefore, the closest point of the line is A - t = decimal(0.0); - - bitsUsedPoints = 1; // 0001 (only A is used) - } + computeClosestPointOnSegment(mPoints[0], mPoints[1], bitsUsedPoints, t); // Compute the closest point mClosestSuppPointA = mSuppPointsA[0] + t * (mSuppPointsA[1] - mSuppPointsA[0]); @@ -275,10 +249,11 @@ bool VoronoiSimplex::recomputeClosestPoint() { int bitsUsedVertices = 0; Vector2 baryCoordsAB; Vector2 baryCoordsCD; + bool isDegenerate; // Compute the point closest to the origin on the tetrahedron bool isOutside = computeClosestPointOnTetrahedron(mPoints[0], mPoints[1], mPoints[2], mPoints[3], - bitsUsedVertices, baryCoordsAB, baryCoordsCD); + bitsUsedVertices, baryCoordsAB, baryCoordsCD, isDegenerate); // If the origin is outside the tetrahedron if (isOutside) { @@ -298,22 +273,27 @@ bool VoronoiSimplex::recomputeClosestPoint() { } else { - // The origin is inside the tetrahedron, therefore, the closest point - // is the origin + // If it is a degenerate case + if (isDegenerate) { + mIsClosestPointValid = false; + } + else { - setBarycentricCoords(0.0, 0.0, 0.0, 0.0); + // The origin is inside the tetrahedron, therefore, the closest point + // is the origin - mClosestSuppPointA.setToZero(); - mClosestSuppPointB.setToZero(); - mClosestPoint.setToZero(); + setBarycentricCoords(0.0, 0.0, 0.0, 0.0); - mIsClosestPointValid = true; + mClosestSuppPointA.setToZero(); + mClosestSuppPointB.setToZero(); + mClosestPoint.setToZero(); + mIsClosestPointValid = true; + } break; } mIsClosestPointValid = checkClosestPointValid(); - } break; } @@ -322,11 +302,45 @@ bool VoronoiSimplex::recomputeClosestPoint() { return mIsClosestPointValid; } +// Compute point of a line segment that is closest to the origin +void VoronoiSimplex::computeClosestPointOnSegment(const Vector3& a, const Vector3& b, int& bitUsedVertices, + float& t) const { + + Vector3 AP = -a; + Vector3 AB = b - a; + decimal APDotAB = AP.dot(AB); + + // If the closest point is on the side of A in the direction of B + if (APDotAB > decimal(0.0)) { + decimal lengthABSquare = AB.lengthSquare(); + + // If the closest point is on the segment AB + if (APDotAB < lengthABSquare) { + t = APDotAB / lengthABSquare; + + bitUsedVertices = 3; // 0011 (both A and B are used) + + } + else { // If the origin is on the side of B that is not in the direction of A + // Therefore, the closest point is B + t = decimal(1.0); + + bitUsedVertices = 2; // 0010 (only B is used) + } + } + else { // If the origin is on the side of A that is not in the direction of B + // Therefore, the closest point of the line is A + t = decimal(0.0); + + bitUsedVertices = 1; // 0001 (only A is used) + } +} + // Compute point on a triangle that is closest to the origin /// This implementation is based on the one in the book /// "Real-Time Collision Detection" by Christer Ericson. -void VoronoiSimplex::computeClosestPointOnTriangle(const Vector3& a, const Vector3& b, - const Vector3& c, int& bitsUsedVertices, Vector3& baryCoordsABC) const { +void VoronoiSimplex::computeClosestPointOnTriangle(const Vector3& a, const Vector3& b, const Vector3& c, + int& bitsUsedVertices, Vector3& baryCoordsABC) const { // Check if the origin is in the Voronoi region of vertex A Vector3 ab = b - a; @@ -438,7 +452,9 @@ void VoronoiSimplex::computeClosestPointOnTriangle(const Vector3& a, const Vecto /// This method returns true if the origin is outside the tetrahedron. bool VoronoiSimplex::computeClosestPointOnTetrahedron(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, int& bitsUsedPoints, Vector2& baryCoordsAB, - Vector2& baryCoordsCD) const { + Vector2& baryCoordsCD, bool& isDegenerate) const { + + isDegenerate = false; // Start as if the origin was inside the tetrahedron bitsUsedPoints = 15; // 1111 (A, B, C and D are used) @@ -451,6 +467,15 @@ bool VoronoiSimplex::computeClosestPointOnTetrahedron(const Vector3& a, const Ve int isOriginOutsideFaceADB = testOriginOutsideOfPlane(a, d, b, c); int isOriginOutsideFaceBDC = testOriginOutsideOfPlane(b, d, c, a); + // If we have a degenerate tetrahedron + if (isOriginOutsideFaceABC < 0 || isOriginOutsideFaceACD < 0 || + isOriginOutsideFaceADB < 0 || isOriginOutsideFaceBDC < 0) { + + // The tetrahedron is degenerate + isDegenerate = true; + return false; + } + if (isOriginOutsideFaceABC == 0 && isOriginOutsideFaceACD == 0 && isOriginOutsideFaceADB == 0 && isOriginOutsideFaceBDC == 0) { @@ -574,31 +599,20 @@ int VoronoiSimplex::testOriginOutsideOfPlane(const Vector3& a, const Vector3& b, decimal signp = (-a).dot(n); decimal signd = (d-a).dot(n); - // This assert is raised if the tetrahedron is degenerate (all points on the same plane) + // If the tetrahedron is degenerate (all points on the same plane) // This should never happen because after a point is added into the simplex, the user // of this class must check that the simplex is not affinely dependent using the // isAffinelyDependent() method - assert(signd * signd > epsilon * epsilon); + if(signd * signd < epsilon * epsilon) { + return -1; + } return signp * signd < decimal(0.0); } // Backup the closest point void VoronoiSimplex::backupClosestPointInSimplex(Vector3& v) { - decimal minDistSquare = DECIMAL_LARGEST; - Bits bit; - - for (bit = mAllBits; bit != 0x0; bit--) { - if (isSubset(bit, mAllBits) && isProperSubset(bit)) { - Vector3 u = computeClosestPointForSubset(bit); - decimal distSquare = u.dot(u); - if (distSquare < minDistSquare) { - minDistSquare = distSquare; - mBitsCurrentSimplex = bit; - v = u; - } - } - } + v = mClosestPoint; } // Return the maximum squared length of a point diff --git a/src/collision/narrowphase/GJK/VoronoiSimplex.h b/src/collision/narrowphase/GJK/VoronoiSimplex.h index 9d1ba580..7b6c6614 100644 --- a/src/collision/narrowphase/GJK/VoronoiSimplex.h +++ b/src/collision/narrowphase/GJK/VoronoiSimplex.h @@ -104,6 +104,10 @@ class VoronoiSimplex { /// Return true if the bool checkClosestPointValid() const; + /// Compute point of a line segment that is closest to the origin + void computeClosestPointOnSegment(const Vector3& a, const Vector3& b, int& bitUsedVertices, + float& t) const; + /// Compute point of a triangle that is closest to the origin void computeClosestPointOnTriangle(const Vector3& a, const Vector3& b, const Vector3& c, int& bitsUsedPoints, Vector3& baryCoordsABC) const; @@ -111,7 +115,8 @@ class VoronoiSimplex { /// Compute point of a tetrahedron that is closest to the origin bool computeClosestPointOnTetrahedron(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d, - int& bitsUsedPoints, Vector2& baryCoordsAB, Vector2& baryCoordsCD) const; + int& bitsUsedPoints, Vector2& baryCoordsAB, Vector2& baryCoordsCD, + bool& isDegenerate) const; /// Test if a point is outside of the plane given by the points of the triangle (a, b, c) int testOriginOutsideOfPlane(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d) const; @@ -198,4 +203,6 @@ inline bool VoronoiSimplex::checkClosestPointValid() const { mBarycentricCoords[2] >= decimal(0.0) && mBarycentricCoords[3] >= decimal(0.0); } +} + #endif From 4bad013c91faa9beff8e291280ae68c3e7441176 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 5 Jul 2016 21:34:44 +0200 Subject: [PATCH 4/7] Make GJK/EPA collision detection more robust --- src/body/CollisionBody.h | 2 +- src/collision/CollisionDetection.h | 2 +- .../narrowphase/EPA/EPAAlgorithm.cpp | 47 +++-- src/collision/narrowphase/EPA/EPAAlgorithm.h | 6 +- .../narrowphase/GJK/GJKAlgorithm.cpp | 192 +++++++----------- src/collision/narrowphase/GJK/GJKAlgorithm.h | 2 +- .../narrowphase/GJK/VoronoiSimplex.cpp | 8 +- .../narrowphase/GJK/VoronoiSimplex.h | 2 +- src/collision/shapes/BoxShape.h | 8 + src/collision/shapes/CapsuleShape.h | 8 + src/collision/shapes/CollisionShape.h | 3 + src/collision/shapes/ConcaveShape.h | 10 +- src/collision/shapes/ConeShape.h | 8 + src/collision/shapes/ConvexMeshShape.h | 8 + src/collision/shapes/CylinderShape.h | 8 + src/collision/shapes/SphereShape.h | 8 + src/collision/shapes/TriangleShape.h | 8 + 17 files changed, 187 insertions(+), 143 deletions(-) diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index 2c88d155..f69be6c5 100644 --- a/src/body/CollisionBody.h +++ b/src/body/CollisionBody.h @@ -303,4 +303,4 @@ inline Vector3 CollisionBody::getLocalVector(const Vector3& worldVector) const { } - #endif +#endif diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index 7e81a25f..d5c585e6 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -35,9 +35,9 @@ #include "memory/MemoryAllocator.h" #include "constraint/ContactPoint.h" #include -#include #include #include +#include /// ReactPhysics3D namespace namespace reactphysics3d { diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp index 513ca367..6c94ed5a 100644 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp +++ b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp @@ -81,8 +81,9 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2, /// 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 -void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex, +/// the correct penetration depth. This method returns true if the EPA penetration +/// depth computation has succeeded and false it has failed. +bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex, CollisionShapeInfo shape1Info, const Transform& transform1, CollisionShapeInfo shape2Info, @@ -92,6 +93,8 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()"); + decimal gjkPenDepthSquare = v.lengthSquare(); + assert(shape1Info.collisionShape->isConvex()); assert(shape2Info.collisionShape->isConvex()); @@ -118,7 +121,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple transform1.getOrientation(); // Get the simplex computed previously by the GJK algorithm - unsigned int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points); + int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points); // Compute the tolerance decimal tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint(); @@ -137,7 +140,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple // 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; + return true; case 2: { // The simplex returned by GJK is a line segment d containing the origin. @@ -204,7 +207,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple } else { // The origin is not in the initial polytope - return; + return false; } // The polytope contains now 4 vertices @@ -234,7 +237,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple 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; + return false; } // Associate the edges of neighbouring triangle faces @@ -319,14 +322,14 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple face3 = triangleStore.newTriangle(points, 1, 4, 2); } else { - return; + return false; } // 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; + return false; } // Associate the edges of neighbouring triangle faces @@ -353,7 +356,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple // can run the EPA algorithm. if (nbTriangles == 0) { - return; + return false; } TriangleEPA* triangle = 0; @@ -368,6 +371,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple // 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); @@ -388,7 +392,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple // 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; @@ -427,13 +431,22 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB); Vector3 normal = v.getUnit(); decimal penetrationDepth = v.length(); - assert(penetrationDepth > 0.0); - if (normal.lengthSquare() < MACHINE_EPSILON) return; - - // Create the contact info object - ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, - shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal); + // If the length of the normal vector is too small, skip this contact point + if (normal.lengthSquare() < MACHINE_EPSILON) { + return false; + } - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); + if (penetrationDepth * penetrationDepth > gjkPenDepthSquare && penetrationDepth > 0) { + + // Create the contact info object + ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, + shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal); + + narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); + + return true; + } + + return false; } diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.h b/src/collision/narrowphase/EPA/EPAAlgorithm.h index 1d4b527a..377ccc11 100644 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.h +++ b/src/collision/narrowphase/EPA/EPAAlgorithm.h @@ -27,7 +27,7 @@ #define REACTPHYSICS3D_EPA_ALGORITHM_H // Libraries -#include "collision/narrowphase/GJK/Simplex.h" +#include "collision/narrowphase/GJK/VoronoiSimplex.h" #include "collision/shapes/CollisionShape.h" #include "collision/CollisionShapeInfo.h" #include "constraint/ContactPoint.h" @@ -123,13 +123,13 @@ class EPAAlgorithm { void init(MemoryAllocator* memoryAllocator); /// Compute the penetration depth with EPA algorithm. - void computePenetrationDepthAndContactPoints(const Simplex& simplex, + bool computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex, CollisionShapeInfo shape1Info, const Transform& transform1, CollisionShapeInfo shape2Info, const Transform& transform2, Vector3& v, - NarrowPhaseCallback* narrowPhaseCallback); + NarrowPhaseCallback* narrowPhaseCallback); }; // Add a triangle face in the candidate triangle heap in the EPA algorithm diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index 49db5968..a9419bb5 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -69,6 +69,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, Vector3 pB; // Closest point of object B decimal vDotw; decimal prevDistSquare; + bool contactFound = false; assert(shape1Info.collisionShape->isConvex()); assert(shape2Info.collisionShape->isConvex()); @@ -79,6 +80,8 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, void** shape1CachedCollisionData = shape1Info.cachedCollisionData; void** shape2CachedCollisionData = shape2Info.cachedCollisionData; + bool isPolytopeShape = shape1->isPolyhedron() && shape2->isPolyhedron(); + // Get the local-space to world-space transforms const Transform transform1 = shape1Info.shapeToWorldTransform; const Transform transform2 = shape2Info.shapeToWorldTransform; @@ -98,7 +101,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, assert(margin > 0.0); // Create a simplex set - Simplex simplex; + VoronoiSimplex simplex; // Get the previous point V (last cached separating axis) Vector3 v = mCurrentOverlappingPair->getCachedSeparatingAxis(); @@ -131,31 +134,9 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, // If the objects intersect only in the margins if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) { - // Compute the closet points of both objects (without the margins) - simplex.computeClosestPointsOfAandB(pA, pB); - - // Project those two points on the margins to have the closest points of both - // object with the margins - decimal dist = sqrt(distSquare); - assert(dist > 0.0); - pA = (pA - (shape1->getMargin() / dist) * v); - pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); - - // Compute the contact info - Vector3 normal = transform1.getOrientation() * (-v.getUnit()); - decimal penetrationDepth = margin - dist; - - // Reject the contact if the penetration depth is negative (due too numerical errors) - if (penetrationDepth <= 0.0) return; - - // Create the contact info object - ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, - shape2Info.collisionShape, normal, penetrationDepth, pA, pB); - - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); - - // There is an intersection, therefore we return - return; + // Contact point has been found + contactFound = true; + break; } // Add the new support point to the simplex @@ -164,62 +145,24 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, // If the simplex is affinely dependent if (simplex.isAffinelyDependent()) { - // Compute the closet points of both objects (without the margins) - simplex.computeClosestPointsOfAandB(pA, pB); - - // Project those two points on the margins to have the closest points of both - // object with the margins - decimal dist = sqrt(distSquare); - assert(dist > 0.0); - pA = (pA - (shape1->getMargin() / dist) * v); - pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); - - // Compute the contact info - Vector3 normal = transform1.getOrientation() * (-v.getUnit()); - decimal penetrationDepth = margin - dist; - - // Reject the contact if the penetration depth is negative (due too numerical errors) - if (penetrationDepth <= 0.0) return; - - // Create the contact info object - ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, - shape2Info.collisionShape, normal, penetrationDepth, pA, pB); - - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); - - // There is an intersection, therefore we return - return; + // Contact point has been found + contactFound = true; + break; } // Compute the point of the simplex closest to the origin - // If the computation of the closest point fail + // If the computation of the closest point fails if (!simplex.computeClosestPoint(v)) { - // Compute the closet points of both objects (without the margins) - simplex.computeClosestPointsOfAandB(pA, pB); + // Contact point has been found + contactFound = true; + break; + } - // Project those two points on the margins to have the closest points of both - // object with the margins - decimal dist = sqrt(distSquare); - assert(dist > 0.0); - pA = (pA - (shape1->getMargin() / dist) * v); - pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); - - // Compute the contact info - Vector3 normal = transform1.getOrientation() * (-v.getUnit()); - decimal penetrationDepth = margin - dist; - - // Reject the contact if the penetration depth is negative (due too numerical errors) - if (penetrationDepth <= 0.0) return; - - // Create the contact info object - ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, - shape2Info.collisionShape, normal, penetrationDepth, pA, pB); - - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); - - // There is an intersection, therefore we return - return; + // Closest point is almost the origin, go to EPA algorithm + // Vector v to small to continue computing support points + if (v.lengthSquare() < MACHINE_EPSILON) { + break; } // Store and update the squared distance of the closest point @@ -228,46 +171,65 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, // If the distance to the closest point doesn't improve a lot if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { + simplex.backupClosestPointInSimplex(v); // Get the new squared distance distSquare = v.lengthSquare(); - // Compute the closet points of both objects (without the margins) - simplex.computeClosestPointsOfAandB(pA, pB); + // Contact point has been found + contactFound = true; + break; + } - // Project those two points on the margins to have the closest points of both - // object with the margins - decimal dist = sqrt(distSquare); - assert(dist > 0.0); - pA = (pA - (shape1->getMargin() / dist) * v); - pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); + } while((isPolytopeShape && !simplex.isFull()) || (!isPolytopeShape && !simplex.isFull() && + distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint())); - // Compute the contact info - Vector3 normal = transform1.getOrientation() * (-v.getUnit()); - decimal penetrationDepth = margin - dist; - - // Reject the contact if the penetration depth is negative (due too numerical errors) - if (penetrationDepth <= 0.0) return; - - // Create the contact info object - ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, - shape2Info.collisionShape, normal, penetrationDepth, pA, pB); + bool isEPAResultValid = false; - narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); + // If no contact has been found (penetration case) + if (!contactFound) { - // There is an intersection, therefore we return + // The objects (without margins) intersect. Therefore, we run the GJK algorithm + // again but on the enlarged objects to compute a simplex polytope that contains + // the origin. Then, we give that simplex polytope to the EPA algorithm to compute + // the correct penetration depth and contact points between the enlarged objects. + isEPAResultValid = computePenetrationDepthForEnlargedObjects(shape1Info, transform1, shape2Info, + transform2, narrowPhaseCallback, v); + } + + if ((contactFound || !isEPAResultValid) && distSquare > MACHINE_EPSILON) { + + // Compute the closet points of both objects (without the margins) + simplex.computeClosestPointsOfAandB(pA, pB); + + // Project those two points on the margins to have the closest points of both + // object with the margins + decimal dist = std::sqrt(distSquare); + assert(dist > 0.0); + pA = (pA - (shape1->getMargin() / dist) * v); + pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v); + + // Compute the contact info + Vector3 normal = transform1.getOrientation() * (-v.getUnit()); + decimal penetrationDepth = margin - dist; + + // If the penetration depth is negative (due too numerical errors), there is no contact + if (penetrationDepth <= 0.0) { return; } - } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * - simplex.getMaxLengthSquareOfAPoint()); - // The objects (without margins) intersect. Therefore, we run the GJK algorithm - // again but on the enlarged objects to compute a simplex polytope that contains - // the origin. Then, we give that simplex polytope to the EPA algorithm to compute - // the correct penetration depth and contact points between the enlarged objects. - return computePenetrationDepthForEnlargedObjects(shape1Info, transform1, shape2Info, - transform2, narrowPhaseCallback, v); + // Do not generate a contact point with zero normal length + if (normal.lengthSquare() < MACHINE_EPSILON) { + return; + } + + // Create the contact info object + ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape, + shape2Info.collisionShape, normal, penetrationDepth, pA, pB); + + narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo); + } } /// This method runs the GJK algorithm on the two enlarged objects (with margin) @@ -275,7 +237,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info, /// assumed to intersect in the original objects (without margin). Therefore such /// a polytope must exist. Then, we give that polytope to the EPA algorithm to /// compute the correct penetration depth and contact points of the enlarged objects. -void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info, +bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info, const Transform& transform1, const CollisionShapeInfo& shape2Info, const Transform& transform2, @@ -283,7 +245,7 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap Vector3& v) { PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()"); - Simplex simplex; + VoronoiSimplex simplex; Vector3 suppA; Vector3 suppB; Vector3 w; @@ -297,6 +259,8 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap const ConvexShape* shape1 = static_cast(shape1Info.collisionShape); const ConvexShape* shape2 = static_cast(shape2Info.collisionShape); + bool isPolytopeShape = shape1->isPolyhedron() && shape2->isPolyhedron(); + void** shape1CachedCollisionData = shape1Info.cachedCollisionData; void** shape2CachedCollisionData = shape2Info.cachedCollisionData; @@ -322,18 +286,18 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap if (vDotw > 0.0) { // No intersection, we return - return; + return false; } // Add the new support point to the simplex simplex.addPoint(w, suppA, suppB); if (simplex.isAffinelyDependent()) { - return; + return false; } if (!simplex.computeClosestPoint(v)) { - return; + return false; } // Store and update the square distance @@ -341,11 +305,11 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap distSquare = v.lengthSquare(); if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) { - return; + return false; } - } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * - simplex.getMaxLengthSquareOfAPoint()); + } while((!simplex.isFull() && isPolytopeShape) || (!isPolytopeShape && !simplex.isFull() && + distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint())); // Give the simplex computed with GJK algorithm to the EPA algorithm // which will compute the correct penetration depth and contact points @@ -372,7 +336,7 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS const Vector3 suppB(localPoint); // Create a simplex set - Simplex simplex; + VoronoiSimplex simplex; // Initial supporting direction Vector3 v(1, 1, 1); @@ -446,7 +410,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& Vector3 w; // Create a simplex set - Simplex simplex; + VoronoiSimplex simplex; Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0)); decimal lambda = decimal(0.0); diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.h b/src/collision/narrowphase/GJK/GJKAlgorithm.h index 23ebc789..8617a002 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.h +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.h @@ -75,7 +75,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm { GJKAlgorithm& operator=(const GJKAlgorithm& algorithm); /// Compute the penetration depth for enlarged objects. - void computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info, + bool computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info, const Transform& transform1, const CollisionShapeInfo& shape2Info, const Transform& transform2, diff --git a/src/collision/narrowphase/GJK/VoronoiSimplex.cpp b/src/collision/narrowphase/GJK/VoronoiSimplex.cpp index a30f52ff..382ccc9b 100644 --- a/src/collision/narrowphase/GJK/VoronoiSimplex.cpp +++ b/src/collision/narrowphase/GJK/VoronoiSimplex.cpp @@ -150,10 +150,7 @@ bool VoronoiSimplex::isAffinelyDependent() const { /// pA = sum(lambda_i * a_i) where "a_i" are the support points of object A /// pB = sum(lambda_i * b_i) where "b_i" are the support points of object B /// with lambda_i = deltaX_i / deltaX -void VoronoiSimplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) { - - // Recompute the closest point (if necessary) - //recomputeClosestPoint(); +void VoronoiSimplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const { pA = mClosestSuppPointA; pB = mClosestSuppPointB; @@ -380,6 +377,7 @@ void VoronoiSimplex::computeClosestPointOnTriangle(const Vector3& a, const Vecto // The origin is in the Voronoi region of edge AB // We return the projection of the origin on the edge AB + assert(std::abs(d1 - d3) > MACHINE_EPSILON); decimal v = d1 / (d1 - d3); // Set the barycentric coords of the closest point on the triangle @@ -410,6 +408,7 @@ void VoronoiSimplex::computeClosestPointOnTriangle(const Vector3& a, const Vecto // The origin is in the Voronoi region of edge AC // We return the projection of the origin on the edge AC + assert(std::abs(d2 - d6) > MACHINE_EPSILON); decimal w = d2 / (d2 - d6); // Set the barycentric coords of the closest point on the triangle @@ -425,6 +424,7 @@ void VoronoiSimplex::computeClosestPointOnTriangle(const Vector3& a, const Vecto // The origin is in the Voronoi region of edge BC // We return the projection of the origin on the edge BC + assert(std::abs((d4 - d3) + (d5 - d6)) > MACHINE_EPSILON); decimal w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); // Set the barycentric coords of the closest point on the triangle diff --git a/src/collision/narrowphase/GJK/VoronoiSimplex.h b/src/collision/narrowphase/GJK/VoronoiSimplex.h index 7b6c6614..9cbee47e 100644 --- a/src/collision/narrowphase/GJK/VoronoiSimplex.h +++ b/src/collision/narrowphase/GJK/VoronoiSimplex.h @@ -165,7 +165,7 @@ class VoronoiSimplex { void backupClosestPointInSimplex(Vector3& point); /// Compute the closest points "pA" and "pB" of object A and B. - void computeClosestPointsOfAandB(Vector3& pA, Vector3& pB); + void computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const; /// Compute the closest point to the origin of the current simplex. bool computeClosestPoint(Vector3& v); diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index f581baa0..bda4bf3f 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -99,6 +99,9 @@ class BoxShape : public ConvexShape { /// Return the local bounds of the shape in x, y and z directions virtual void getLocalBounds(Vector3& min, Vector3& max) const; + /// Return true if the collision shape is a polyhedron + virtual bool isPolyhedron() const; + /// Return the local inertia tensor of the collision shape virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; }; @@ -134,6 +137,11 @@ inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const { min = -max; } +// Return true if the collision shape is a polyhedron +inline bool BoxShape::isPolyhedron() const { + return true; +} + // Return the number of bytes used by the collision shape inline size_t BoxShape::getSizeInBytes() const { return sizeof(BoxShape); diff --git a/src/collision/shapes/CapsuleShape.h b/src/collision/shapes/CapsuleShape.h index 56269d04..8e77f050 100644 --- a/src/collision/shapes/CapsuleShape.h +++ b/src/collision/shapes/CapsuleShape.h @@ -101,6 +101,9 @@ class CapsuleShape : public ConvexShape { /// Return the local bounds of the shape in x, y and z directions virtual void getLocalBounds(Vector3& min, Vector3& max) const; + /// Return true if the collision shape is a polyhedron + virtual bool isPolyhedron() const; + /// Return the local inertia tensor of the collision shape virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; }; @@ -154,6 +157,11 @@ inline void CapsuleShape::getLocalBounds(Vector3& min, Vector3& max) const { min.z = min.x; } +// Return true if the collision shape is a polyhedron +inline bool CapsuleShape::isPolyhedron() const { + return false; +} + // Return a local support point in a given direction without the object margin. /// A capsule is the convex hull of two spheres S1 and S2. The support point in the direction "d" /// of the convex hull of a set of convex objects is the support point "p" in the set of all diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index cd89255d..80419c03 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -98,6 +98,9 @@ class CollisionShape { /// Return true if the collision shape is convex, false if it is concave virtual bool isConvex() const=0; + /// Return true if the collision shape is a polyhedron + virtual bool isPolyhedron() const=0; + /// Return the local bounds of the shape in x, y and z directions virtual void getLocalBounds(Vector3& min, Vector3& max) const=0; diff --git a/src/collision/shapes/ConcaveShape.h b/src/collision/shapes/ConcaveShape.h index 9c1418df..9cee92ab 100644 --- a/src/collision/shapes/ConcaveShape.h +++ b/src/collision/shapes/ConcaveShape.h @@ -101,6 +101,9 @@ class ConcaveShape : public CollisionShape { /// Return true if the collision shape is convex, false if it is concave virtual bool isConvex() const; + /// Return true if the collision shape is a polyhedron + virtual bool isPolyhedron() const; + /// Use a callback method on all triangles of the concave shape inside a given AABB virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const=0; @@ -116,11 +119,16 @@ inline decimal ConcaveShape::getTriangleMargin() const { return mTriangleMargin; } -/// Return true if the collision shape is convex, false if it is concave +// Return true if the collision shape is convex, false if it is concave inline bool ConcaveShape::isConvex() const { return false; } +// Return true if the collision shape is a polyhedron +inline bool ConcaveShape::isPolyhedron() const { + return true; +} + // Return true if a point is inside the collision shape inline bool ConcaveShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const { return false; diff --git a/src/collision/shapes/ConeShape.h b/src/collision/shapes/ConeShape.h index b693f386..53630817 100644 --- a/src/collision/shapes/ConeShape.h +++ b/src/collision/shapes/ConeShape.h @@ -107,6 +107,9 @@ class ConeShape : public ConvexShape { /// Return the local bounds of the shape in x, y and z directions virtual void getLocalBounds(Vector3& min, Vector3& max) const; + /// Return true if the collision shape is a polyhedron + virtual bool isPolyhedron() const; + /// Return the local inertia tensor of the collision shape virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; }; @@ -159,6 +162,11 @@ inline void ConeShape::getLocalBounds(Vector3& min, Vector3& max) const { min.z = min.x; } +// Return true if the collision shape is a polyhedron +inline bool ConeShape::isPolyhedron() const { + return false; +} + // Return the local inertia tensor of the collision shape /** * @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h index d23e91b0..94ec67f9 100644 --- a/src/collision/shapes/ConvexMeshShape.h +++ b/src/collision/shapes/ConvexMeshShape.h @@ -140,6 +140,9 @@ class ConvexMeshShape : public ConvexShape { /// Add an edge into the convex mesh by specifying the two vertex indices of the edge. void addEdge(uint v1, uint v2); + /// Return true if the collision shape is a polyhedron + virtual bool isPolyhedron() const; + /// Return true if the edges information is used to speed up the collision detection bool isEdgesInformationUsed() const; @@ -159,6 +162,11 @@ inline size_t ConvexMeshShape::getSizeInBytes() const { return sizeof(ConvexMeshShape); } +// Return true if the collision shape is a polyhedron +inline bool ConvexMeshShape::isPolyhedron() const { + return true; +} + // Return the local bounds of the shape in x, y and z directions /** * @param min The minimum bounds of the shape in local-space coordinates diff --git a/src/collision/shapes/CylinderShape.h b/src/collision/shapes/CylinderShape.h index 71e4f9ae..a732ecf6 100644 --- a/src/collision/shapes/CylinderShape.h +++ b/src/collision/shapes/CylinderShape.h @@ -98,6 +98,9 @@ class CylinderShape : public ConvexShape { /// Return the height decimal getHeight() const; + /// Return true if the collision shape is a polyhedron + virtual bool isPolyhedron() const; + /// Set the scaling vector of the collision shape virtual void setLocalScaling(const Vector3& scaling); @@ -124,6 +127,11 @@ inline decimal CylinderShape::getHeight() const { return mHalfHeight + mHalfHeight; } +// Return true if the collision shape is a polyhedron +inline bool CylinderShape::isPolyhedron() const { + return false; +} + // Set the scaling vector of the collision shape inline void CylinderShape::setLocalScaling(const Vector3& scaling) { diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h index a1316ae0..bfbe46b5 100644 --- a/src/collision/shapes/SphereShape.h +++ b/src/collision/shapes/SphereShape.h @@ -83,6 +83,9 @@ class SphereShape : public ConvexShape { /// Return the radius of the sphere decimal getRadius() const; + /// Return true if the collision shape is a polyhedron + virtual bool isPolyhedron() const; + /// Set the scaling vector of the collision shape virtual void setLocalScaling(const Vector3& scaling); @@ -104,6 +107,11 @@ inline decimal SphereShape::getRadius() const { return mMargin; } +// Return true if the collision shape is a polyhedron +inline bool SphereShape::isPolyhedron() const { + return false; +} + // Set the scaling vector of the collision shape inline void SphereShape::setLocalScaling(const Vector3& scaling) { diff --git a/src/collision/shapes/TriangleShape.h b/src/collision/shapes/TriangleShape.h index 1b9f3681..71af0f89 100644 --- a/src/collision/shapes/TriangleShape.h +++ b/src/collision/shapes/TriangleShape.h @@ -116,6 +116,9 @@ class TriangleShape : public ConvexShape { /// Return the coordinates of a given vertex of the triangle Vector3 getVertex(int index) const; + /// Return true if the collision shape is a polyhedron + virtual bool isPolyhedron() const; + // ---------- Friendship ---------- // friend class ConcaveMeshRaycastCallback; @@ -218,6 +221,11 @@ inline Vector3 TriangleShape::getVertex(int index) const { return mPoints[index]; } +// Return true if the collision shape is a polyhedron +inline bool TriangleShape::isPolyhedron() const { + return true; +} + } #endif From da9f6ae23347c9a63628aad231970e99e93c78f4 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Tue, 5 Jul 2016 22:02:16 +0200 Subject: [PATCH 5/7] Remove Simplex class (replaced by VoronoiSimplex) --- CMakeLists.txt | 4 +- .../narrowphase/GJK/GJKAlgorithm.cpp | 1 - src/collision/narrowphase/GJK/Simplex.cpp | 394 ------------------ src/collision/narrowphase/GJK/Simplex.h | 189 --------- 4 files changed, 2 insertions(+), 586 deletions(-) delete mode 100644 src/collision/narrowphase/GJK/Simplex.cpp delete mode 100644 src/collision/narrowphase/GJK/Simplex.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 6850a981..e85693d7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -75,8 +75,8 @@ SET (REACTPHYSICS3D_SOURCES "src/collision/narrowphase/EPA/TriangleEPA.cpp" "src/collision/narrowphase/EPA/TrianglesStore.h" "src/collision/narrowphase/EPA/TrianglesStore.cpp" - "src/collision/narrowphase/GJK/Simplex.h" - "src/collision/narrowphase/GJK/Simplex.cpp" + "src/collision/narrowphase/GJK/VoronoiSimplex.h" + "src/collision/narrowphase/GJK/VoronoiSimplex.cpp" "src/collision/narrowphase/GJK/GJKAlgorithm.h" "src/collision/narrowphase/GJK/GJKAlgorithm.cpp" "src/collision/narrowphase/NarrowPhaseAlgorithm.h" diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index a9419bb5..89c4f459 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -25,7 +25,6 @@ // Libraries #include "GJKAlgorithm.h" -#include "Simplex.h" #include "constraint/ContactPoint.h" #include "configuration.h" #include "engine/Profiler.h" diff --git a/src/collision/narrowphase/GJK/Simplex.cpp b/src/collision/narrowphase/GJK/Simplex.cpp deleted file mode 100644 index 45e72e69..00000000 --- a/src/collision/narrowphase/GJK/Simplex.cpp +++ /dev/null @@ -1,394 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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 "Simplex.h" -#include - -// We want to use the ReactPhysics3D namespace -using namespace reactphysics3d; - -// Constructor -Simplex::Simplex() : mBitsCurrentSimplex(0x0), mAllBits(0x0) { - -} - -// Destructor -Simplex::~Simplex() { - -} - -// Add a new support point of (A-B) into the simplex -/// suppPointA : support point of object A in a direction -v -/// suppPointB : support point of object B in a direction v -/// point : support point of object (A-B) => point = suppPointA - suppPointB -void Simplex::addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB) { - assert(!isFull()); - - mLastFound = 0; - mLastFoundBit = 0x1; - - // Look for the bit corresponding to one of the four point that is not in - // the current simplex - while (overlap(mBitsCurrentSimplex, mLastFoundBit)) { - mLastFound++; - mLastFoundBit <<= 1; - } - - assert(mLastFound < 4); - - // Add the point into the simplex - mPoints[mLastFound] = point; - mPointsLengthSquare[mLastFound] = point.dot(point); - mAllBits = mBitsCurrentSimplex | mLastFoundBit; - - // Update the cached values - updateCache(); - - // Compute the cached determinant values - computeDeterminants(); - - // Add the support points of objects A and B - mSuppPointsA[mLastFound] = suppPointA; - mSuppPointsB[mLastFound] = suppPointB; -} - -// Return true if the point is in the simplex -bool Simplex::isPointInSimplex(const Vector3& point) const { - int i; - Bits bit; - - // For each four possible points in the simplex - for (i=0, bit = 0x1; i<4; i++, bit <<= 1) { - // Check if the current point is in the simplex - if (overlap(mAllBits, bit) && point == mPoints[i]) return true; - } - - return false; -} - -// Update the cached values used during the GJK algorithm -void Simplex::updateCache() { - int i; - Bits bit; - - // For each of the four possible points of the simplex - for (i=0, bit = 0x1; i<4; i++, bit <<= 1) { - // If the current points is in the simplex - if (overlap(mBitsCurrentSimplex, bit)) { - - // Compute the distance between two points in the possible simplex set - mDiffLength[i][mLastFound] = mPoints[i] - mPoints[mLastFound]; - mDiffLength[mLastFound][i] = -mDiffLength[i][mLastFound]; - - // Compute the squared length of the vector - // distances from points in the possible simplex set - mNormSquare[i][mLastFound] = mNormSquare[mLastFound][i] = - mDiffLength[i][mLastFound].dot(mDiffLength[i][mLastFound]); - } - } -} - -// Return the points of the simplex -unsigned int Simplex::getSimplex(Vector3* suppPointsA, Vector3* suppPointsB, - Vector3* points) const { - unsigned int nbVertices = 0; - int i; - Bits bit; - - // For each four point in the possible simplex - for (i=0, bit=0x1; i<4; i++, bit <<=1) { - - // If the current point is in the simplex - if (overlap(mBitsCurrentSimplex, bit)) { - - // Store the points - suppPointsA[nbVertices] = this->mSuppPointsA[nbVertices]; - suppPointsB[nbVertices] = this->mSuppPointsB[nbVertices]; - points[nbVertices] = this->mPoints[nbVertices]; - - nbVertices++; - } - } - - // Return the number of points in the simplex - return nbVertices; -} - -// Compute the cached determinant values -void Simplex::computeDeterminants() { - mDet[mLastFoundBit][mLastFound] = 1.0; - - // If the current simplex is not empty - if (!isEmpty()) { - int i; - Bits bitI; - - // For each possible four points in the simplex set - for (i=0, bitI = 0x1; i<4; i++, bitI <<= 1) { - - // If the current point is in the simplex - if (overlap(mBitsCurrentSimplex, bitI)) { - Bits bit2 = bitI | mLastFoundBit; - - mDet[bit2][i] = mDiffLength[mLastFound][i].dot(mPoints[mLastFound]); - mDet[bit2][mLastFound] = mDiffLength[i][mLastFound].dot(mPoints[i]); - - - int j; - Bits bitJ; - - for (j=0, bitJ = 0x1; j 0 for each "i" in I_x and -/// 2. delta(X U {y_j})_j <= 0 for each "j" not in I_x_ -bool Simplex::isValidSubset(Bits subset) const { - int i; - Bits bit; - - // For each four point in the possible simplex set - for (i=0, bit=0x1; i<4; i++, bit <<= 1) { - if (overlap(mAllBits, bit)) { - // If the current point is in the subset - if (overlap(subset, bit)) { - // If one delta(X)_i is smaller or equal to zero - if (mDet[subset][i] <= 0.0) { - // The subset is not valid - return false; - } - } - // If the point is not in the subset and the value delta(X U {y_j})_j - // is bigger than zero - else if (mDet[subset | bit][i] > 0.0) { - // The subset is not valid - return false; - } - } - } - - return true; -} - -// Compute the closest points "pA" and "pB" of object A and B. -/// The points are computed as follows : -/// pA = sum(lambda_i * a_i) where "a_i" are the support points of object A -/// pB = sum(lambda_i * b_i) where "b_i" are the support points of object B -/// with lambda_i = deltaX_i / deltaX -void Simplex::computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const { - decimal deltaX = 0.0; - pA.setAllValues(0.0, 0.0, 0.0); - pB.setAllValues(0.0, 0.0, 0.0); - int i; - Bits bit; - - // For each four points in the possible simplex set - for (i=0, bit=0x1; i<4; i++, bit <<= 1) { - // If the current point is part of the simplex - if (overlap(mBitsCurrentSimplex, bit)) { - deltaX += mDet[mBitsCurrentSimplex][i]; - pA += mDet[mBitsCurrentSimplex][i] * mSuppPointsA[i]; - pB += mDet[mBitsCurrentSimplex][i] * mSuppPointsB[i]; - } - } - - assert(deltaX > 0.0); - decimal factor = decimal(1.0) / deltaX; - pA *= factor; - pB *= factor; -} - -// Compute the closest point "v" to the origin of the current simplex. -/// This method executes the Jonhnson's algorithm for computing the point -/// "v" of simplex that is closest to the origin. The method returns true -/// if a closest point has been found. -bool Simplex::computeClosestPoint(Vector3& v) { - Bits subset; - - // For each possible simplex set - for (subset=mBitsCurrentSimplex; subset != 0x0; subset--) { - // If the simplex is a subset of the current simplex and is valid for the Johnson's - // algorithm test - if (isSubset(subset, mBitsCurrentSimplex) && isValidSubset(subset | mLastFoundBit)) { - mBitsCurrentSimplex = subset | mLastFoundBit; // Add the last added point to the current simplex - v = computeClosestPointForSubset(mBitsCurrentSimplex); // Compute the closest point in the simplex - return true; - } - } - - // If the simplex that contains only the last added point is valid for the Johnson's algorithm test - if (isValidSubset(mLastFoundBit)) { - mBitsCurrentSimplex = mLastFoundBit; // Set the current simplex to the set that contains only the last added point - mMaxLengthSquare = mPointsLengthSquare[mLastFound]; // Update the maximum square length - v = mPoints[mLastFound]; // The closest point of the simplex "v" is the last added point - return true; - } - - // The algorithm failed to found a point - return false; -} - -// Backup the closest point -void Simplex::backupClosestPointInSimplex(Vector3& v) { - decimal minDistSquare = DECIMAL_LARGEST; - Bits bit; - - for (bit = mAllBits; bit != 0x0; bit--) { - if (isSubset(bit, mAllBits) && isProperSubset(bit)) { - Vector3 u = computeClosestPointForSubset(bit); - decimal distSquare = u.dot(u); - if (distSquare < minDistSquare) { - minDistSquare = distSquare; - mBitsCurrentSimplex = bit; - v = u; - } - } - } -} - -// Return the closest point "v" in the convex hull of the points in the subset -// represented by the bits "subset" -Vector3 Simplex::computeClosestPointForSubset(Bits subset) { - Vector3 v(0.0, 0.0, 0.0); // Closet point v = sum(lambda_i * points[i]) - mMaxLengthSquare = 0.0; - decimal deltaX = 0.0; // deltaX = sum of all det[subset][i] - int i; - Bits bit; - - // For each four point in the possible simplex set - for (i=0, bit=0x1; i<4; i++, bit <<= 1) { - // If the current point is in the subset - if (overlap(subset, bit)) { - // deltaX = sum of all det[subset][i] - deltaX += mDet[subset][i]; - - if (mMaxLengthSquare < mPointsLengthSquare[i]) { - mMaxLengthSquare = mPointsLengthSquare[i]; - } - - // Closest point v = sum(lambda_i * points[i]) - v += mDet[subset][i] * mPoints[i]; - } - } - - assert(deltaX > 0.0); - - // Return the closet point "v" in the convex hull for the given subset - return (decimal(1.0) / deltaX) * v; -} diff --git a/src/collision/narrowphase/GJK/Simplex.h b/src/collision/narrowphase/GJK/Simplex.h deleted file mode 100644 index 72f618bf..00000000 --- a/src/collision/narrowphase/GJK/Simplex.h +++ /dev/null @@ -1,189 +0,0 @@ -/******************************************************************************** -* ReactPhysics3D physics library, http://www.reactphysics3d.com * -* Copyright (c) 2010-2016 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. * -* * -********************************************************************************/ - -#ifndef REACTPHYSICS3D_SIMPLEX_H -#define REACTPHYSICS3D_SIMPLEX_H - -// Libraries -#include "mathematics/mathematics.h" -#include - -/// ReactPhysics3D namespace -namespace reactphysics3d { - -// Type definitions -typedef unsigned int Bits; - -// Class Simplex -/** - * This class represents a simplex which is a set of 3D points. This - * class is used in the GJK algorithm. This implementation is based on - * the implementation discussed in the book "Collision Detection in 3D - * Environments". This class implements the Johnson's algorithm for - * computing the point of a simplex that is closest to the origin and also - * the smallest simplex needed to represent that closest point. - */ -class Simplex { - - private: - - // -------------------- Attributes -------------------- // - - /// Current points - Vector3 mPoints[4]; - - /// pointsLengthSquare[i] = (points[i].length)^2 - decimal mPointsLengthSquare[4]; - - /// Maximum length of pointsLengthSquare[i] - decimal mMaxLengthSquare; - - /// Support points of object A in local coordinates - Vector3 mSuppPointsA[4]; - - /// Support points of object B in local coordinates - Vector3 mSuppPointsB[4]; - - /// diff[i][j] contains points[i] - points[j] - Vector3 mDiffLength[4][4]; - - /// Cached determinant values - decimal mDet[16][4]; - - /// norm[i][j] = (diff[i][j].length())^2 - decimal mNormSquare[4][4]; - - /// 4 bits that identify the current points of the simplex - /// For instance, 0101 means that points[1] and points[3] are in the simplex - Bits mBitsCurrentSimplex; - - /// Number between 1 and 4 that identify the last found support point - Bits mLastFound; - - /// Position of the last found support point (lastFoundBit = 0x1 << lastFound) - Bits mLastFoundBit; - - /// allBits = bitsCurrentSimplex | lastFoundBit; - Bits mAllBits; - - // -------------------- Methods -------------------- // - - /// Private copy-constructor - Simplex(const Simplex& simplex); - - /// Private assignment operator - Simplex& operator=(const Simplex& simplex); - - /// Return true if some bits of "a" overlap with bits of "b" - bool overlap(Bits a, Bits b) const; - - /// Return true if the bits of "b" is a subset of the bits of "a" - bool isSubset(Bits a, Bits b) const; - - /// Return true if the subset is a valid one for the closest point computation. - bool isValidSubset(Bits subset) const; - - /// Return true if the subset is a proper subset. - bool isProperSubset(Bits subset) const; - - /// Update the cached values used during the GJK algorithm - void updateCache(); - - /// Compute the cached determinant values - void computeDeterminants(); - - /// Return the closest point "v" in the convex hull of a subset of points - Vector3 computeClosestPointForSubset(Bits subset); - - public: - - // -------------------- Methods -------------------- // - - /// Constructor - Simplex(); - - /// Destructor - ~Simplex(); - - /// Return true if the simplex contains 4 points - bool isFull() const; - - /// Return true if the simplex is empty - bool isEmpty() const; - - /// Return the points of the simplex - unsigned int getSimplex(Vector3* mSuppPointsA, Vector3* mSuppPointsB, - Vector3* mPoints) const; - - /// Return the maximum squared length of a point - decimal getMaxLengthSquareOfAPoint() const; - - /// Add a new support point of (A-B) into the simplex. - void addPoint(const Vector3& point, const Vector3& suppPointA, const Vector3& suppPointB); - - /// Return true if the point is in the simplex - bool isPointInSimplex(const Vector3& point) const; - - /// Return true if the set is affinely dependent - bool isAffinelyDependent() const; - - /// Backup the closest point - void backupClosestPointInSimplex(Vector3& point); - - /// Compute the closest points "pA" and "pB" of object A and B. - void computeClosestPointsOfAandB(Vector3& pA, Vector3& pB) const; - - /// Compute the closest point to the origin of the current simplex. - bool computeClosestPoint(Vector3& v); -}; - -// Return true if some bits of "a" overlap with bits of "b" -inline bool Simplex::overlap(Bits a, Bits b) const { - return ((a & b) != 0x0); -} - -// Return true if the bits of "b" is a subset of the bits of "a" -inline bool Simplex::isSubset(Bits a, Bits b) const { - return ((a & b) == a); -} - -// Return true if the simplex contains 4 points -inline bool Simplex::isFull() const { - return (mBitsCurrentSimplex == 0xf); -} - -// Return true if the simple is empty -inline bool Simplex::isEmpty() const { - return (mBitsCurrentSimplex == 0x0); -} - -// Return the maximum squared length of a point -inline decimal Simplex::getMaxLengthSquareOfAPoint() const { - return mMaxLengthSquare; -} - -} - -#endif From 9cc633fc67db1e193092d462e259b22ba843b3b6 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 6 Jul 2016 06:48:19 +0200 Subject: [PATCH 6/7] Modify initial GJK support direction --- src/engine/OverlappingPair.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp index fa45817e..9c4814b5 100644 --- a/src/engine/OverlappingPair.cpp +++ b/src/engine/OverlappingPair.cpp @@ -33,7 +33,7 @@ using namespace reactphysics3d; OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, int nbMaxContactManifolds, MemoryAllocator& memoryAllocator) : mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds), - mCachedSeparatingAxis(1.0, 1.0, 1.0) { + mCachedSeparatingAxis(0.0, 1.0, 0.0) { } From 4e70174da725e0e207620619cee52aa2ff80652b Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Wed, 20 Jul 2016 07:18:17 +0200 Subject: [PATCH 7/7] Use default nb solver iterations in demo scenes --- testbed/scenes/collisionshapes/CollisionShapesScene.cpp | 3 --- testbed/scenes/concavemesh/ConcaveMeshScene.cpp | 3 --- testbed/scenes/cubes/CubesScene.cpp | 3 --- testbed/scenes/heightfield/HeightFieldScene.cpp | 3 --- testbed/scenes/joints/JointsScene.cpp | 3 --- 5 files changed, 15 deletions(-) diff --git a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp index bcf7c836..e18feea9 100644 --- a/testbed/scenes/collisionshapes/CollisionShapesScene.cpp +++ b/testbed/scenes/collisionshapes/CollisionShapesScene.cpp @@ -48,9 +48,6 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name) // Create the dynamics world for the physics simulation mDynamicsWorld = new rp3d::DynamicsWorld(gravity); - // Set the number of iterations of the constraint solver - mDynamicsWorld->setNbIterationsVelocitySolver(15); - float radius = 3.0f; for (int i=0; isetNbIterationsVelocitySolver(15); - // ---------- Create the boxes ----------- // for (int i=0; isetNbIterationsVelocitySolver(15); - float radius = 2.0f; // Create all the cubes of the scene diff --git a/testbed/scenes/heightfield/HeightFieldScene.cpp b/testbed/scenes/heightfield/HeightFieldScene.cpp index ab168b13..8779fa9c 100644 --- a/testbed/scenes/heightfield/HeightFieldScene.cpp +++ b/testbed/scenes/heightfield/HeightFieldScene.cpp @@ -45,9 +45,6 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC // Create the dynamics world for the physics simulation mDynamicsWorld = new rp3d::DynamicsWorld(gravity); - // Set the number of iterations of the constraint solver - mDynamicsWorld->setNbIterationsVelocitySolver(15); - // ---------- Create the boxes ----------- // // For each box diff --git a/testbed/scenes/joints/JointsScene.cpp b/testbed/scenes/joints/JointsScene.cpp index ae006573..9927613f 100644 --- a/testbed/scenes/joints/JointsScene.cpp +++ b/testbed/scenes/joints/JointsScene.cpp @@ -47,9 +47,6 @@ JointsScene::JointsScene(const std::string& name) // Create the dynamics world for the physics simulation mDynamicsWorld = new rp3d::DynamicsWorld(gravity); - // Set the number of iterations of the constraint solver - mDynamicsWorld->setNbIterationsVelocitySolver(15); - // Create the Ball-and-Socket joint createBallAndSocketJoints();