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

Remove Simplex class (replaced by VoronoiSimplex)

Browse Source
This commit is contained in:
Daniel Chappuis 2016-07-05 22:02:16 +02:00
parent 4bad013c91
commit da9f6ae233
4 changed files with 2 additions and 586 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

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

1
src/collision/narrowphase/GJK/GJKAlgorithm.cpp
View File

@ -25,7 +25,6 @@
// Libraries
#include "GJKAlgorithm.h"
#include "Simplex.h"
#include "constraint/ContactPoint.h"
#include "configuration.h"
#include "engine/Profiler.h"

394
src/collision/narrowphase/GJK/Simplex.cpp
View File

@ -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 <cfloat>
// 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<i; j++, bitJ <<= 1) {
if (overlap(mBitsCurrentSimplex, bitJ)) {
int k;
Bits bit3 = bitJ | bit2;
k = mNormSquare[i][j] < mNormSquare[mLastFound][j] ? i : mLastFound;
mDet[bit3][j] = mDet[bit2][i] * mDiffLength[k][j].dot(mPoints[i]) +
mDet[bit2][mLastFound] *
mDiffLength[k][j].dot(mPoints[mLastFound]);
k = mNormSquare[j][i] < mNormSquare[mLastFound][i] ? j : mLastFound;
mDet[bit3][i] = mDet[bitJ | mLastFoundBit][j] *
mDiffLength[k][i].dot(mPoints[j]) +
mDet[bitJ | mLastFoundBit][mLastFound] *
mDiffLength[k][i].dot(mPoints[mLastFound]);
k = mNormSquare[i][mLastFound] < mNormSquare[j][mLastFound] ? i : j;
mDet[bit3][mLastFound] = mDet[bitJ | bitI][j] *
mDiffLength[k][mLastFound].dot(mPoints[j]) +
mDet[bitJ | bitI][i] *
mDiffLength[k][mLastFound].dot(mPoints[i]);
}
}
}
}
if (mAllBits == 0xf) {
int k;
k = mNormSquare[1][0] < mNormSquare[2][0] ?
(mNormSquare[1][0] < mNormSquare[3][0] ? 1 : 3) :
(mNormSquare[2][0] < mNormSquare[3][0] ? 2 : 3);
mDet[0xf][0] = mDet[0xe][1] * mDiffLength[k][0].dot(mPoints[1]) +
mDet[0xe][2] * mDiffLength[k][0].dot(mPoints[2]) +
mDet[0xe][3] * mDiffLength[k][0].dot(mPoints[3]);
k = mNormSquare[0][1] < mNormSquare[2][1] ?
(mNormSquare[0][1] < mNormSquare[3][1] ? 0 : 3) :
(mNormSquare[2][1] < mNormSquare[3][1] ? 2 : 3);
mDet[0xf][1] = mDet[0xd][0] * mDiffLength[k][1].dot(mPoints[0]) +
mDet[0xd][2] * mDiffLength[k][1].dot(mPoints[2]) +
mDet[0xd][3] * mDiffLength[k][1].dot(mPoints[3]);
k = mNormSquare[0][2] < mNormSquare[1][2] ?
(mNormSquare[0][2] < mNormSquare[3][2] ? 0 : 3) :
(mNormSquare[1][2] < mNormSquare[3][2] ? 1 : 3);
mDet[0xf][2] = mDet[0xb][0] * mDiffLength[k][2].dot(mPoints[0]) +
mDet[0xb][1] * mDiffLength[k][2].dot(mPoints[1]) +
mDet[0xb][3] * mDiffLength[k][2].dot(mPoints[3]);
k = mNormSquare[0][3] < mNormSquare[1][3] ?
(mNormSquare[0][3] < mNormSquare[2][3] ? 0 : 2) :
(mNormSquare[1][3] < mNormSquare[2][3] ? 1 : 2);
mDet[0xf][3] = mDet[0x7][0] * mDiffLength[k][3].dot(mPoints[0]) +
mDet[0x7][1] * mDiffLength[k][3].dot(mPoints[1]) +
mDet[0x7][2] * mDiffLength[k][3].dot(mPoints[2]);
}
}
}
// Return true if the subset is a proper subset.
/// A proper subset X is a subset where for all point "y_i" in X, we have
/// detX_i value bigger than zero
bool Simplex::isProperSubset(Bits subset) const {
int i;
Bits bit;
// For each four point of the possible simplex set
for (i=0, bit=0x1; i<4; i++, bit <<=1) {
if (overlap(subset, bit) && mDet[subset][i] <= 0.0) {
return false;
}
}
return true;
}
// 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 Simplex::isAffinelyDependent() const {
decimal sum = 0.0;
int i;
Bits bit;
// For each four point of the possible simplex set
for (i=0, bit=0x1; i<4; i++, bit <<= 1) {
if (overlap(mAllBits, bit)) {
sum += mDet[mAllBits][i];
}
}
return (sum <= 0.0);
}
// Return true if the subset is a valid one for the closest point computation.
/// A subset X is valid if :
/// 1. delta(X)_i > 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;
}

189
src/collision/narrowphase/GJK/Simplex.h
View File

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