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

Add VoronoiSimplex class for GJK algorithm

Browse Source
This commit is contained in:
Daniel Chappuis 2016-06-20 08:41:22 +02:00
parent 9fae1b4e35
commit fd224ebaba
2 changed files with 819 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

618
src/collision/narrowphase/GJK/VoronoiSimplex.cpp Normal file
View File

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

201
src/collision/narrowphase/GJK/VoronoiSimplex.h Normal file
View File

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