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