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

Merge branch 'voronoi_simplex' into develop

Browse Source
This commit is contained in:
Daniel Chappuis 2016-08-21 11:09:39 +02:00
commit 68a169efbd
26 changed files with 1028 additions and 740 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"

2
src/body/CollisionBody.h
View File

@ -303,4 +303,4 @@ inline Vector3 CollisionBody::getLocalVector(const Vector3& worldVector) const {
}
#endif
#endif

5
src/collision/CollisionDetection.h
View File

@ -35,9 +35,9 @@
#include "memory/MemoryAllocator.h"
#include "constraint/ContactPoint.h"
#include <vector>
#include <map>
#include <set>
#include <utility>
#include <map>
/// ReactPhysics3D namespace
namespace reactphysics3d {
@ -62,6 +62,9 @@ class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback {
}
// Destructor
virtual ~TestCollisionBetweenShapesCallback() { }
// Called by a narrow-phase collision algorithm when a new contact has been found
virtual void notifyContact(OverlappingPair* overlappingPair,
const ContactPointInfo& contactInfo);

47
src/collision/narrowphase/EPA/EPAAlgorithm.cpp
View File

@ -81,8 +81,9 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
/// enlarged objects (with margin) where the original objects (without margin)
/// intersect. An initial simplex that contains origin has been computed with
/// GJK algorithm. The EPA Algorithm will extend this simplex polytope to find
/// the correct penetration depth
void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex,
/// the correct penetration depth. This method returns true if the EPA penetration
/// depth computation has succeeded and false it has failed.
bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex,
CollisionShapeInfo shape1Info,
const Transform& transform1,
CollisionShapeInfo shape2Info,
@ -92,6 +93,8 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()");
decimal gjkPenDepthSquare = v.lengthSquare();
assert(shape1Info.collisionShape->isConvex());
assert(shape2Info.collisionShape->isConvex());
@ -118,7 +121,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
transform1.getOrientation();
// Get the simplex computed previously by the GJK algorithm
unsigned int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
// Compute the tolerance
decimal tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint();
@ -137,7 +140,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// Only one point in the simplex (which should be the origin).
// We have a touching contact with zero penetration depth.
// We drop that kind of contact. Therefore, we return false
return;
return true;
case 2: {
// The simplex returned by GJK is a line segment d containing the origin.
@ -204,7 +207,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
}
else {
// The origin is not in the initial polytope
return;
return false;
}
// The polytope contains now 4 vertices
@ -234,7 +237,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL)
&& face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0
&& face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
return;
return false;
}
// Associate the edges of neighbouring triangle faces
@ -319,14 +322,14 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
face3 = triangleStore.newTriangle(points, 1, 4, 2);
}
else {
return;
return false;
}
// If the constructed tetrahedron is not correct
if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL)
&& face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0
&& face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
return;
return false;
}
// Associate the edges of neighbouring triangle faces
@ -353,7 +356,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// can run the EPA algorithm.
if (nbTriangles == 0) {
return;
return false;
}
TriangleEPA* triangle = 0;
@ -368,6 +371,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// If the candidate face in the heap is not obsolete
if (!triangle->getIsObsolete()) {
// If we have reached the maximum number of support points
if (nbVertices == MAX_SUPPORT_POINTS) {
assert(false);
@ -388,7 +392,7 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// Update the upper bound of the penetration depth
decimal wDotv = points[indexNewVertex].dot(triangle->getClosestPoint());
assert(wDotv > 0.0);
decimal wDotVSquare = wDotv * wDotv / triangle->getDistSquare();
if (wDotVSquare < upperBoundSquarePenDepth) {
upperBoundSquarePenDepth = wDotVSquare;
@ -427,13 +431,22 @@ void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB);
Vector3 normal = v.getUnit();
decimal penetrationDepth = v.length();
assert(penetrationDepth > 0.0);
if (normal.lengthSquare() < MACHINE_EPSILON) return;
// Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal);
// If the length of the normal vector is too small, skip this contact point
if (normal.lengthSquare() < MACHINE_EPSILON) {
return false;
}
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
if (penetrationDepth * penetrationDepth > gjkPenDepthSquare && penetrationDepth > 0) {
// Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
return true;
}
return false;
}

6
src/collision/narrowphase/EPA/EPAAlgorithm.h
View File

@ -27,7 +27,7 @@
#define REACTPHYSICS3D_EPA_ALGORITHM_H
// Libraries
#include "collision/narrowphase/GJK/Simplex.h"
#include "collision/narrowphase/GJK/VoronoiSimplex.h"
#include "collision/shapes/CollisionShape.h"
#include "collision/CollisionShapeInfo.h"
#include "constraint/ContactPoint.h"
@ -123,13 +123,13 @@ class EPAAlgorithm {
void init(MemoryAllocator* memoryAllocator);
/// Compute the penetration depth with EPA algorithm.
void computePenetrationDepthAndContactPoints(const Simplex& simplex,
bool computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex,
CollisionShapeInfo shape1Info,
const Transform& transform1,
CollisionShapeInfo shape2Info,
const Transform& transform2,
Vector3& v,
NarrowPhaseCallback* narrowPhaseCallback);
NarrowPhaseCallback* narrowPhaseCallback);
};
// Add a triangle face in the candidate triangle heap in the EPA algorithm

193
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"
@ -69,6 +68,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
Vector3 pB; // Closest point of object B
decimal vDotw;
decimal prevDistSquare;
bool contactFound = false;
assert(shape1Info.collisionShape->isConvex());
assert(shape2Info.collisionShape->isConvex());
@ -79,6 +79,8 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
bool isPolytopeShape = shape1->isPolyhedron() && shape2->isPolyhedron();
// Get the local-space to world-space transforms
const Transform transform1 = shape1Info.shapeToWorldTransform;
const Transform transform2 = shape2Info.shapeToWorldTransform;
@ -98,7 +100,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
assert(margin > 0.0);
// Create a simplex set
Simplex simplex;
VoronoiSimplex simplex;
// Get the previous point V (last cached separating axis)
Vector3 v = mCurrentOverlappingPair->getCachedSeparatingAxis();
@ -131,31 +133,9 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
// If the objects intersect only in the margins
if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) {
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both
// object with the margins
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return;
// Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return
return;
// Contact point has been found
contactFound = true;
break;
}
// Add the new support point to the simplex
@ -164,62 +144,24 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
// If the simplex is affinely dependent
if (simplex.isAffinelyDependent()) {
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both
// object with the margins
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return;
// Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return
return;
// Contact point has been found
contactFound = true;
break;
}
// Compute the point of the simplex closest to the origin
// If the computation of the closest point fail
// If the computation of the closest point fails
if (!simplex.computeClosestPoint(v)) {
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
// Contact point has been found
contactFound = true;
break;
}
// Project those two points on the margins to have the closest points of both
// object with the margins
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return;
// Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return
return;
// Closest point is almost the origin, go to EPA algorithm
// Vector v to small to continue computing support points
if (v.lengthSquare() < MACHINE_EPSILON) {
break;
}
// Store and update the squared distance of the closest point
@ -228,46 +170,65 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
// If the distance to the closest point doesn't improve a lot
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
simplex.backupClosestPointInSimplex(v);
// Get the new squared distance
distSquare = v.lengthSquare();
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
// Contact point has been found
contactFound = true;
break;
}
// Project those two points on the margins to have the closest points of both
// object with the margins
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
} while((isPolytopeShape && !simplex.isFull()) || (!isPolytopeShape && !simplex.isFull() &&
distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint()));
// Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return;
// Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
bool isEPAResultValid = false;
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// If no contact has been found (penetration case)
if (!contactFound) {
// There is an intersection, therefore we return
// The objects (without margins) intersect. Therefore, we run the GJK algorithm
// again but on the enlarged objects to compute a simplex polytope that contains
// the origin. Then, we give that simplex polytope to the EPA algorithm to compute
// the correct penetration depth and contact points between the enlarged objects.
isEPAResultValid = computePenetrationDepthForEnlargedObjects(shape1Info, transform1, shape2Info,
transform2, narrowPhaseCallback, v);
}
if ((contactFound || !isEPAResultValid) && distSquare > MACHINE_EPSILON) {
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
// Project those two points on the margins to have the closest points of both
// object with the margins
decimal dist = std::sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// If the penetration depth is negative (due too numerical errors), there is no contact
if (penetrationDepth <= 0.0) {
return;
}
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
simplex.getMaxLengthSquareOfAPoint());
// The objects (without margins) intersect. Therefore, we run the GJK algorithm
// again but on the enlarged objects to compute a simplex polytope that contains
// the origin. Then, we give that simplex polytope to the EPA algorithm to compute
// the correct penetration depth and contact points between the enlarged objects.
return computePenetrationDepthForEnlargedObjects(shape1Info, transform1, shape2Info,
transform2, narrowPhaseCallback, v);
// Do not generate a contact point with zero normal length
if (normal.lengthSquare() < MACHINE_EPSILON) {
return;
}
// Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
}
}
/// This method runs the GJK algorithm on the two enlarged objects (with margin)
@ -275,7 +236,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
/// assumed to intersect in the original objects (without margin). Therefore such
/// a polytope must exist. Then, we give that polytope to the EPA algorithm to
/// compute the correct penetration depth and contact points of the enlarged objects.
void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
const Transform& transform1,
const CollisionShapeInfo& shape2Info,
const Transform& transform2,
@ -283,7 +244,7 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
Vector3& v) {
PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()");
Simplex simplex;
VoronoiSimplex simplex;
Vector3 suppA;
Vector3 suppB;
Vector3 w;
@ -297,6 +258,8 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
bool isPolytopeShape = shape1->isPolyhedron() && shape2->isPolyhedron();
void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
@ -322,18 +285,18 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
if (vDotw > 0.0) {
// No intersection, we return
return;
return false;
}
// Add the new support point to the simplex
simplex.addPoint(w, suppA, suppB);
if (simplex.isAffinelyDependent()) {
return;
return false;
}
if (!simplex.computeClosestPoint(v)) {
return;
return false;
}
// Store and update the square distance
@ -341,11 +304,11 @@ void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap
distSquare = v.lengthSquare();
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
return;
return false;
}
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
simplex.getMaxLengthSquareOfAPoint());
} while((!simplex.isFull() && isPolytopeShape) || (!isPolytopeShape && !simplex.isFull() &&
distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint()));
// Give the simplex computed with GJK algorithm to the EPA algorithm
// which will compute the correct penetration depth and contact points
@ -372,7 +335,7 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
const Vector3 suppB(localPoint);
// Create a simplex set
Simplex simplex;
VoronoiSimplex simplex;
// Initial supporting direction
Vector3 v(1, 1, 1);
@ -446,7 +409,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
Vector3 w;
// Create a simplex set
Simplex simplex;
VoronoiSimplex simplex;
Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0));
decimal lambda = decimal(0.0);

2
src/collision/narrowphase/GJK/GJKAlgorithm.h
View File

@ -75,7 +75,7 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
GJKAlgorithm& operator=(const GJKAlgorithm& algorithm);
/// Compute the penetration depth for enlarged objects.
void computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
bool computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
const Transform& transform1,
const CollisionShapeInfo& shape2Info,
const Transform& transform2,

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

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

@ -0,0 +1,632 @@
/********************************************************************************
* 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] = mSuppPointsB[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
// Test in three different ways (for more robustness)
case 4: return std::abs((mPoints[1] - mPoints[0]).dot((mPoints[2] - mPoints[0]).cross(mPoints[3] - mPoints[0]))) <= epsilon;
}
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) const {
pA = mClosestSuppPointA;
pB = mClosestSuppPointB;
}
// 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;
float t;
// The simplex is a line AB (where A=mPoints[0] and B=mPoints[1].
// We need to find the point of that line closest to the origin
computeClosestPointOnSegment(mPoints[0], mPoints[1], bitsUsedPoints, t);
// 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;
bool isDegenerate;
// Compute the point closest to the origin on the tetrahedron
bool isOutside = computeClosestPointOnTetrahedron(mPoints[0], mPoints[1], mPoints[2], mPoints[3],
bitsUsedVertices, baryCoordsAB, baryCoordsCD, isDegenerate);
// 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 {
// If it is a degenerate case
if (isDegenerate) {
mIsClosestPointValid = false;
}
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 of a line segment that is closest to the origin
void VoronoiSimplex::computeClosestPointOnSegment(const Vector3& a, const Vector3& b, int& bitUsedVertices,
float& t) const {
Vector3 AP = -a;
Vector3 AB = b - a;
decimal APDotAB = AP.dot(AB);
// If the closest point is on the side of A in the direction of B
if (APDotAB > decimal(0.0)) {
decimal lengthABSquare = AB.lengthSquare();
// If the closest point is on the segment AB
if (APDotAB < lengthABSquare) {
t = APDotAB / lengthABSquare;
bitUsedVertices = 3; // 0011 (both A and B are used)
}
else { // If the origin is on the side of B that is not in the direction of A
// Therefore, the closest point is B
t = decimal(1.0);
bitUsedVertices = 2; // 0010 (only B is used)
}
}
else { // If the origin is on the side of A that is not in the direction of B
// Therefore, the closest point of the line is A
t = decimal(0.0);
bitUsedVertices = 1; // 0001 (only A is used)
}
}
// Compute point on a triangle that is closest to the origin
/// This implementation is based on the one in the book
/// "Real-Time Collision Detection" by Christer Ericson.
void VoronoiSimplex::computeClosestPointOnTriangle(const Vector3& a, const Vector3& b, const Vector3& c,
int& bitsUsedVertices, Vector3& baryCoordsABC) const {
// 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
assert(std::abs(d1 - d3) > MACHINE_EPSILON);
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
assert(std::abs(d2 - d6) > MACHINE_EPSILON);
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
assert(std::abs((d4 - d3) + (d5 - d6)) > MACHINE_EPSILON);
decimal w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
// Set the barycentric coords of the closest point on the triangle
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, bool& isDegenerate) const {
isDegenerate = false;
// 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 we have a degenerate tetrahedron
if (isOriginOutsideFaceABC < 0 || isOriginOutsideFaceACD < 0 ||
isOriginOutsideFaceADB < 0 || isOriginOutsideFaceBDC < 0) {
// The tetrahedron is degenerate
isDegenerate = true;
return false;
}
if (isOriginOutsideFaceABC == 0 && isOriginOutsideFaceACD == 0 &&
isOriginOutsideFaceADB == 0 && isOriginOutsideFaceBDC == 0) {
// 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);
// 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
if(signd * signd < epsilon * epsilon) {
return -1;
}
return signp * signd < decimal(0.0);
}
// Backup the closest point
void VoronoiSimplex::backupClosestPointInSimplex(Vector3& v) {
v = mClosestPoint;
}
// 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;
}

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

@ -0,0 +1,208 @@
/********************************************************************************
* 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 line segment that is closest to the origin
void computeClosestPointOnSegment(const Vector3& a, const Vector3& b, int& bitUsedVertices,
float& t) const;
/// Compute point of a triangle that is closest to the origin
void computeClosestPointOnTriangle(const Vector3& a, const Vector3& b,
const Vector3& c, int& bitsUsedPoints, Vector3& baryCoordsABC) const;
/// 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,
bool& isDegenerate) const;
/// Test if a point is outside of the plane given by the points of the triangle (a, b, c)
int testOriginOutsideOfPlane(const Vector3& a, const Vector3& b, const Vector3& c, const Vector3& d) const;
/// 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) const;
/// 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

8
src/collision/shapes/BoxShape.h
View File

@ -99,6 +99,9 @@ class BoxShape : public ConvexShape {
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// Return true if the collision shape is a polyhedron
virtual bool isPolyhedron() const;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
};
@ -134,6 +137,11 @@ inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const {
min = -max;
}
// Return true if the collision shape is a polyhedron
inline bool BoxShape::isPolyhedron() const {
return true;
}
// Return the number of bytes used by the collision shape
inline size_t BoxShape::getSizeInBytes() const {
return sizeof(BoxShape);

8
src/collision/shapes/CapsuleShape.h
View File

@ -101,6 +101,9 @@ class CapsuleShape : public ConvexShape {
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// Return true if the collision shape is a polyhedron
virtual bool isPolyhedron() const;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
};
@ -154,6 +157,11 @@ inline void CapsuleShape::getLocalBounds(Vector3& min, Vector3& max) const {
min.z = min.x;
}
// Return true if the collision shape is a polyhedron
inline bool CapsuleShape::isPolyhedron() const {
return false;
}
// Return a local support point in a given direction without the object margin.
/// A capsule is the convex hull of two spheres S1 and S2. The support point in the direction "d"
/// of the convex hull of a set of convex objects is the support point "p" in the set of all

3
src/collision/shapes/CollisionShape.h
View File

@ -98,6 +98,9 @@ class CollisionShape {
/// Return true if the collision shape is convex, false if it is concave
virtual bool isConvex() const=0;
/// Return true if the collision shape is a polyhedron
virtual bool isPolyhedron() const=0;
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const=0;

10
src/collision/shapes/ConcaveShape.h
View File

@ -101,6 +101,9 @@ class ConcaveShape : public CollisionShape {
/// Return true if the collision shape is convex, false if it is concave
virtual bool isConvex() const;
/// Return true if the collision shape is a polyhedron
virtual bool isPolyhedron() const;
/// Use a callback method on all triangles of the concave shape inside a given AABB
virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const=0;
@ -116,11 +119,16 @@ inline decimal ConcaveShape::getTriangleMargin() const {
return mTriangleMargin;
}
/// Return true if the collision shape is convex, false if it is concave
// Return true if the collision shape is convex, false if it is concave
inline bool ConcaveShape::isConvex() const {
return false;
}
// Return true if the collision shape is a polyhedron
inline bool ConcaveShape::isPolyhedron() const {
return true;
}
// Return true if a point is inside the collision shape
inline bool ConcaveShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
return false;

8
src/collision/shapes/ConeShape.h
View File

@ -107,6 +107,9 @@ class ConeShape : public ConvexShape {
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// Return true if the collision shape is a polyhedron
virtual bool isPolyhedron() const;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
};
@ -159,6 +162,11 @@ inline void ConeShape::getLocalBounds(Vector3& min, Vector3& max) const {
min.z = min.x;
}
// Return true if the collision shape is a polyhedron
inline bool ConeShape::isPolyhedron() const {
return false;
}
// Return the local inertia tensor of the collision shape
/**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space

8
src/collision/shapes/ConvexMeshShape.h
View File

@ -140,6 +140,9 @@ class ConvexMeshShape : public ConvexShape {
/// Add an edge into the convex mesh by specifying the two vertex indices of the edge.
void addEdge(uint v1, uint v2);
/// Return true if the collision shape is a polyhedron
virtual bool isPolyhedron() const;
/// Return true if the edges information is used to speed up the collision detection
bool isEdgesInformationUsed() const;
@ -159,6 +162,11 @@ inline size_t ConvexMeshShape::getSizeInBytes() const {
return sizeof(ConvexMeshShape);
}
// Return true if the collision shape is a polyhedron
inline bool ConvexMeshShape::isPolyhedron() const {
return true;
}
// Return the local bounds of the shape in x, y and z directions
/**
* @param min The minimum bounds of the shape in local-space coordinates

8
src/collision/shapes/CylinderShape.h
View File

@ -98,6 +98,9 @@ class CylinderShape : public ConvexShape {
/// Return the height
decimal getHeight() const;
/// Return true if the collision shape is a polyhedron
virtual bool isPolyhedron() const;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
@ -124,6 +127,11 @@ inline decimal CylinderShape::getHeight() const {
return mHalfHeight + mHalfHeight;
}
// Return true if the collision shape is a polyhedron
inline bool CylinderShape::isPolyhedron() const {
return false;
}
// Set the scaling vector of the collision shape
inline void CylinderShape::setLocalScaling(const Vector3& scaling) {

8
src/collision/shapes/SphereShape.h
View File

@ -83,6 +83,9 @@ class SphereShape : public ConvexShape {
/// Return the radius of the sphere
decimal getRadius() const;
/// Return true if the collision shape is a polyhedron
virtual bool isPolyhedron() const;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
@ -104,6 +107,11 @@ inline decimal SphereShape::getRadius() const {
return mMargin;
}
// Return true if the collision shape is a polyhedron
inline bool SphereShape::isPolyhedron() const {
return false;
}
// Set the scaling vector of the collision shape
inline void SphereShape::setLocalScaling(const Vector3& scaling) {

8
src/collision/shapes/TriangleShape.h
View File

@ -116,6 +116,9 @@ class TriangleShape : public ConvexShape {
/// Return the coordinates of a given vertex of the triangle
Vector3 getVertex(int index) const;
/// Return true if the collision shape is a polyhedron
virtual bool isPolyhedron() const;
// ---------- Friendship ---------- //
friend class ConcaveMeshRaycastCallback;
@ -218,6 +221,11 @@ inline Vector3 TriangleShape::getVertex(int index) const {
return mPoints[index];
}
// Return true if the collision shape is a polyhedron
inline bool TriangleShape::isPolyhedron() const {
return true;
}
}
#endif

2
src/engine/OverlappingPair.cpp
View File

@ -33,7 +33,7 @@ using namespace reactphysics3d;
OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
int nbMaxContactManifolds, MemoryAllocator& memoryAllocator)
: mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds),
mCachedSeparatingAxis(1.0, 1.0, 1.0) {
mCachedSeparatingAxis(0.0, 1.0, 0.0) {
}

3
testbed/scenes/collisionshapes/CollisionShapesScene.cpp
View File

@ -48,9 +48,6 @@ CollisionShapesScene::CollisionShapesScene(const std::string& name)
// Create the dynamics world for the physics simulation
mDynamicsWorld = new rp3d::DynamicsWorld(gravity);
// Set the number of iterations of the constraint solver
mDynamicsWorld->setNbIterationsVelocitySolver(15);
float radius = 3.0f;
for (int i=0; i<NB_COMPOUND_SHAPES; i++) {

3
testbed/scenes/concavemesh/ConcaveMeshScene.cpp
View File

@ -48,9 +48,6 @@ ConcaveMeshScene::ConcaveMeshScene(const std::string& name)
// Create the dynamics world for the physics simulation
mDynamicsWorld = new rp3d::DynamicsWorld(gravity);
// Set the number of iterations of the constraint solver
mDynamicsWorld->setNbIterationsVelocitySolver(15);
// ---------- Create the boxes ----------- //
for (int i=0; i<NB_BOXES_X; i++) {

3
testbed/scenes/cubes/CubesScene.cpp
View File

@ -46,9 +46,6 @@ CubesScene::CubesScene(const std::string& name)
// Create the dynamics world for the physics simulation
mDynamicsWorld = new rp3d::DynamicsWorld(gravity);
// Set the number of iterations of the constraint solver
mDynamicsWorld->setNbIterationsVelocitySolver(15);
float radius = 2.0f;
// Create all the cubes of the scene

3
testbed/scenes/heightfield/HeightFieldScene.cpp
View File

@ -45,9 +45,6 @@ HeightFieldScene::HeightFieldScene(const std::string& name) : SceneDemo(name, SC
// Create the dynamics world for the physics simulation
mDynamicsWorld = new rp3d::DynamicsWorld(gravity);
// Set the number of iterations of the constraint solver
mDynamicsWorld->setNbIterationsVelocitySolver(15);
// ---------- Create the boxes ----------- //
// For each box

3
testbed/scenes/joints/JointsScene.cpp
View File

@ -47,9 +47,6 @@ JointsScene::JointsScene(const std::string& name)
// Create the dynamics world for the physics simulation
mDynamicsWorld = new rp3d::DynamicsWorld(gravity);
// Set the number of iterations of the constraint solver
mDynamicsWorld->setNbIterationsVelocitySolver(15);
// Create the Ball-and-Socket joint
createBallAndSocketJoints();