From fd224ebabae357f5578e435198b7ec253efd52a9 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Mon, 20 Jun 2016 08:41:22 +0200
Subject: [PATCH] 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 <cfloat>
+
+// 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<mNbPoints; i++) {
+
+ // Compute the distance between the points
+ decimal distanceSquare = (mPoints[i] - point).lengthSquare();
+
+ // If the point is very close
+ if (distanceSquare <= epsilon) {
+ return true;
+ }
+ }
+
+ return false;
+}
+
+// Return the points of the simplex
+int VoronoiSimplex::getSimplex(Vector3* suppPointsA, Vector3* suppPointsB,
+ Vector3* points) const {
+
+ for (int i=0; i<mNbPoints; i++) {
+ points[i] = mPoints[i];
+ suppPointsA[i] = mSuppPointsA[i];
+ suppPointsB[i] = mSuppPointsB[i];
+ }
+
+ // Return the number of points in the simplex
+ return mNbPoints;
+}
+
+// Return true if the set is affinely dependent.
+/// A set if affinely dependent if a point of the set
+/// is an affine combination of other points in the set
+bool VoronoiSimplex::isAffinelyDependent() const {
+
+ assert(mNbPoints >= 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<mNbPoints; i++) {
+ decimal lengthSquare = mPoints[i].lengthSquare();
+ if (lengthSquare > 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 <vector>
+
+/// 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