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

Merge branch 'optimization' into develop

Browse Source
This commit is contained in:
Daniel Chappuis 2018-01-14 11:05:21 +01:00
commit 27a451adcf
204 changed files with 16055 additions and 10782 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
CMakeLists.txt
View File

@ -60,6 +60,9 @@ SET (REACTPHYSICS3D_SOURCES
"src/body/CollisionBody.cpp"
"src/body/RigidBody.h"
"src/body/RigidBody.cpp"
"src/collision/ContactPointInfo.h"
"src/collision/ContactManifoldInfo.h"
"src/collision/ContactManifoldInfo.cpp"
"src/collision/broadphase/BroadPhaseAlgorithm.h"
"src/collision/broadphase/BroadPhaseAlgorithm.cpp"
"src/collision/broadphase/DynamicAABBTree.h"
@ -67,28 +70,31 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/narrowphase/CollisionDispatch.h"
"src/collision/narrowphase/DefaultCollisionDispatch.h"
"src/collision/narrowphase/DefaultCollisionDispatch.cpp"
"src/collision/narrowphase/EPA/EdgeEPA.h"
"src/collision/narrowphase/EPA/EdgeEPA.cpp"
"src/collision/narrowphase/EPA/EPAAlgorithm.h"
"src/collision/narrowphase/EPA/EPAAlgorithm.cpp"
"src/collision/narrowphase/EPA/TriangleEPA.h"
"src/collision/narrowphase/EPA/TriangleEPA.cpp"
"src/collision/narrowphase/EPA/TrianglesStore.h"
"src/collision/narrowphase/EPA/TrianglesStore.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/SAT/SATAlgorithm.h"
"src/collision/narrowphase/SAT/SATAlgorithm.cpp"
"src/collision/narrowphase/NarrowPhaseAlgorithm.h"
"src/collision/narrowphase/NarrowPhaseAlgorithm.cpp"
"src/collision/narrowphase/SphereVsSphereAlgorithm.h"
"src/collision/narrowphase/SphereVsSphereAlgorithm.cpp"
"src/collision/narrowphase/ConcaveVsConvexAlgorithm.h"
"src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp"
"src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h"
"src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp"
"src/collision/narrowphase/SphereVsCapsuleAlgorithm.h"
"src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp"
"src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h"
"src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp"
"src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h"
"src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp"
"src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h"
"src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp"
"src/collision/shapes/AABB.h"
"src/collision/shapes/AABB.cpp"
"src/collision/shapes/ConvexShape.h"
"src/collision/shapes/ConvexShape.cpp"
"src/collision/shapes/ConvexPolyhedronShape.h"
"src/collision/shapes/ConvexPolyhedronShape.cpp"
"src/collision/shapes/ConcaveShape.h"
"src/collision/shapes/ConcaveShape.cpp"
"src/collision/shapes/BoxShape.h"
@ -97,12 +103,8 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/shapes/CapsuleShape.cpp"
"src/collision/shapes/CollisionShape.h"
"src/collision/shapes/CollisionShape.cpp"
"src/collision/shapes/ConeShape.h"
"src/collision/shapes/ConeShape.cpp"
"src/collision/shapes/ConvexMeshShape.h"
"src/collision/shapes/ConvexMeshShape.cpp"
"src/collision/shapes/CylinderShape.h"
"src/collision/shapes/CylinderShape.cpp"
"src/collision/shapes/SphereShape.h"
"src/collision/shapes/SphereShape.cpp"
"src/collision/shapes/TriangleShape.h"
@ -117,15 +119,24 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/ProxyShape.cpp"
"src/collision/TriangleVertexArray.h"
"src/collision/TriangleVertexArray.cpp"
"src/collision/PolygonVertexArray.h"
"src/collision/PolygonVertexArray.cpp"
"src/collision/TriangleMesh.h"
"src/collision/TriangleMesh.cpp"
"src/collision/PolyhedronMesh.h"
"src/collision/PolyhedronMesh.cpp"
"src/collision/HalfEdgeStructure.h"
"src/collision/HalfEdgeStructure.cpp"
"src/collision/CollisionDetection.h"
"src/collision/CollisionDetection.cpp"
"src/collision/CollisionShapeInfo.h"
"src/collision/NarrowPhaseInfo.h"
"src/collision/NarrowPhaseInfo.cpp"
"src/collision/ContactManifold.h"
"src/collision/ContactManifold.cpp"
"src/collision/ContactManifoldSet.h"
"src/collision/ContactManifoldSet.cpp"
"src/collision/MiddlePhaseTriangleCallback.h"
"src/collision/MiddlePhaseTriangleCallback.cpp"
"src/constraint/BallAndSocketJoint.h"
"src/constraint/BallAndSocketJoint.cpp"
"src/constraint/ContactPoint.h"
@ -147,7 +158,6 @@ SET (REACTPHYSICS3D_SOURCES
"src/engine/DynamicsWorld.h"
"src/engine/DynamicsWorld.cpp"
"src/engine/EventListener.h"
"src/engine/Impulse.h"
"src/engine/Island.h"
"src/engine/Island.cpp"
"src/engine/Material.h"
@ -158,6 +168,9 @@ SET (REACTPHYSICS3D_SOURCES
"src/engine/Profiler.cpp"
"src/engine/Timer.h"
"src/engine/Timer.cpp"
"src/collision/CollisionCallback.h"
"src/collision/CollisionCallback.cpp"
"src/collision/OverlapCallback.h"
"src/mathematics/mathematics.h"
"src/mathematics/mathematics_functions.h"
"src/mathematics/mathematics_functions.cpp"
@ -175,8 +188,17 @@ SET (REACTPHYSICS3D_SOURCES
"src/mathematics/Ray.h"
"src/mathematics/Vector3.cpp"
"src/memory/MemoryAllocator.h"
"src/memory/MemoryAllocator.cpp"
"src/memory/Stack.h"
"src/memory/PoolAllocator.h"
"src/memory/PoolAllocator.cpp"
"src/memory/SingleFrameAllocator.h"
"src/memory/SingleFrameAllocator.cpp"
"src/memory/DefaultAllocator.h"
"src/memory/MemoryManager.h"
"src/memory/MemoryManager.cpp"
"src/containers/Stack.h"
"src/containers/LinkedList.h"
"src/containers/List.h"
"src/containers/Map.h"
)
# Create the library

51
src/body/CollisionBody.cpp
View File

@ -70,9 +70,16 @@ ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape,
const Transform& transform) {
// Create a new proxy collision shape to attach the collision shape to the body
ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate(
ProxyShape* proxyShape = new (mWorld.mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
sizeof(ProxyShape))) ProxyShape(this, collisionShape,
transform, decimal(1));
transform, decimal(1), mWorld.mMemoryManager);
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
proxyShape->setProfiler(mProfiler);
#endif
// Add it to the list of proxy collision shapes of the body
if (mProxyCollisionShapes == nullptr) {
@ -111,12 +118,12 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
if (current == proxyShape) {
mProxyCollisionShapes = current->mNext;
if (mIsActive) {
if (mIsActive && proxyShape->mBroadPhaseID != -1) {
mWorld.mCollisionDetection.removeProxyCollisionShape(current);
}
current->~ProxyShape();
mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape));
mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, current, sizeof(ProxyShape));
mNbCollisionShapes--;
return;
}
@ -131,12 +138,12 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
ProxyShape* elementToRemove = current->mNext;
current->mNext = elementToRemove->mNext;
if (mIsActive) {
if (mIsActive && proxyShape->mBroadPhaseID != -1) {
mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove);
}
elementToRemove->~ProxyShape();
mWorld.mMemoryAllocator.release(elementToRemove, sizeof(ProxyShape));
mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, elementToRemove, sizeof(ProxyShape));
mNbCollisionShapes--;
return;
}
@ -157,12 +164,12 @@ void CollisionBody::removeAllCollisionShapes() {
// Remove the proxy collision shape
ProxyShape* nextElement = current->mNext;
if (mIsActive) {
if (mIsActive && current->mBroadPhaseID != -1) {
mWorld.mCollisionDetection.removeProxyCollisionShape(current);
}
current->~ProxyShape();
mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape));
mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, current, sizeof(ProxyShape));
// Get the next element in the list
current = nextElement;
@ -177,11 +184,11 @@ void CollisionBody::resetContactManifoldsList() {
// Delete the linked list of contact manifolds of that body
ContactManifoldListElement* currentElement = mContactManifoldsList;
while (currentElement != nullptr) {
ContactManifoldListElement* nextElement = currentElement->next;
ContactManifoldListElement* nextElement = currentElement->getNext();
// Delete the current element
currentElement->~ContactManifoldListElement();
mWorld.mMemoryAllocator.release(currentElement, sizeof(ContactManifoldListElement));
mWorld.mMemoryManager.release(MemoryManager::AllocationType::Pool, currentElement, sizeof(ContactManifoldListElement));
currentElement = nextElement;
}
@ -202,12 +209,15 @@ void CollisionBody::updateBroadPhaseState() const {
// Update the broad-phase state of a proxy collision shape of the body
void CollisionBody::updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool forceReinsert) const {
// Recompute the world-space AABB of the collision shape
AABB aabb;
proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform());
if (proxyShape->mBroadPhaseID != -1) {
// Update the broad-phase state for the proxy collision shape
mWorld.mCollisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert);
// Recompute the world-space AABB of the collision shape
AABB aabb;
proxyShape->getCollisionShape()->computeAABB(aabb, mTransform * proxyShape->getLocalToBodyTransform());
// Update the broad-phase state for the proxy collision shape
mWorld.mCollisionDetection.updateProxyCollisionShape(proxyShape, aabb, Vector3(0, 0, 0), forceReinsert) ;
}
}
// Set whether or not the body is active
@ -240,8 +250,11 @@ void CollisionBody::setIsActive(bool isActive) {
// For each proxy shape of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != nullptr; shape = shape->mNext) {
// Remove the proxy shape from the collision detection
mWorld.mCollisionDetection.removeProxyCollisionShape(shape);
if (shape->mBroadPhaseID != -1) {
// Remove the proxy shape from the collision detection
mWorld.mCollisionDetection.removeProxyCollisionShape(shape);
}
}
// Reset the contact manifold list of the body
@ -272,8 +285,8 @@ int CollisionBody::resetIsAlreadyInIslandAndCountManifolds() {
// this body
ContactManifoldListElement* currentElement = mContactManifoldsList;
while (currentElement != nullptr) {
currentElement->contactManifold->mIsAlreadyInIsland = false;
currentElement = currentElement->next;
currentElement->getContactManifold()->mIsAlreadyInIsland = false;
currentElement = currentElement->getNext();
nbManifolds++;
}

38
src/body/CollisionBody.h
View File

@ -34,8 +34,9 @@
#include "collision/shapes/AABB.h"
#include "collision/shapes/CollisionShape.h"
#include "collision/RaycastInfo.h"
#include "memory/MemoryAllocator.h"
#include "memory/PoolAllocator.h"
#include "configuration.h"
#include "engine/Profiler.h"
/// Namespace reactphysics3d
namespace reactphysics3d {
@ -85,6 +86,13 @@ class CollisionBody : public Body {
/// Reference to the world the body belongs to
CollisionWorld& mWorld;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
// -------------------- Methods -------------------- //
/// Reset the contact manifold lists
@ -153,6 +161,9 @@ class CollisionBody : public Body {
/// Raycast method with feedback information
bool raycast(const Ray& ray, RaycastInfo& raycastInfo);
/// Test if the collision body overlaps with a given AABB
bool testAABBOverlap(const AABB& worldAABB) const;
/// Compute and return the AABB of the body by merging all proxy shapes AABBs
AABB getAABB() const;
@ -174,6 +185,13 @@ class CollisionBody : public Body {
/// Return the body local-space coordinates of a vector given in the world-space coordinates
Vector3 getLocalVector(const Vector3& worldVector) const;
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
virtual void setProfiler(Profiler* profiler);
#endif
// -------------------- Friendship -------------------- //
friend class CollisionWorld;
@ -301,6 +319,24 @@ inline Vector3 CollisionBody::getLocalVector(const Vector3& worldVector) const {
return mTransform.getOrientation().getInverse() * worldVector;
}
/// Test if the collision body overlaps with a given AABB
/**
* @param worldAABB The AABB (in world-space coordinates) that will be used to test overlap
* @return True if the given AABB overlaps with the AABB of the collision body
*/
inline bool CollisionBody::testAABBOverlap(const AABB& worldAABB) const {
return worldAABB.testCollision(getAABB());
}
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void CollisionBody::setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
#endif
}
#endif

59
src/body/RigidBody.cpp
View File

@ -39,13 +39,16 @@ using namespace reactphysics3d;
* @param id The ID of the body
*/
RigidBody::RigidBody(const Transform& transform, CollisionWorld& world, bodyindex id)
: CollisionBody(transform, world, id), mInitMass(decimal(1.0)),
: CollisionBody(transform, world, id), mArrayIndex(0), mInitMass(decimal(1.0)),
mCenterOfMassLocal(0, 0, 0), mCenterOfMassWorld(transform.getPosition()),
mIsGravityEnabled(true), mLinearDamping(decimal(0.0)), mAngularDamping(decimal(0.0)),
mJointsList(nullptr) {
// Compute the inverse mass
mMassInverse = decimal(1.0) / mInitMass;
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
}
// Destructor
@ -90,11 +93,15 @@ void RigidBody::setType(BodyType type) {
mMassInverse = decimal(0.0);
mInertiaTensorLocal.setToZero();
mInertiaTensorLocalInverse.setToZero();
mInertiaTensorInverseWorld.setToZero();
}
else { // If it is a dynamic body
mMassInverse = decimal(1.0) / mInitMass;
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
}
// Awake the body
@ -125,6 +132,9 @@ void RigidBody::setInertiaTensorLocal(const Matrix3x3& inertiaTensorLocal) {
// Compute the inverse local inertia tensor
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
}
// Set the local center of mass of the body (in local-space coordinates)
@ -166,7 +176,7 @@ void RigidBody::setMass(decimal mass) {
}
// Remove a joint from the joints list
void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint) {
void RigidBody::removeJointFromJointsList(MemoryManager& memoryManager, const Joint* joint) {
assert(joint != nullptr);
assert(mJointsList != nullptr);
@ -176,7 +186,8 @@ void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, cons
JointListElement* elementToRemove = mJointsList;
mJointsList = elementToRemove->next;
elementToRemove->~JointListElement();
memoryAllocator.release(elementToRemove, sizeof(JointListElement));
memoryManager.release(MemoryManager::AllocationType::Pool,
elementToRemove, sizeof(JointListElement));
}
else { // If the element to remove is not the first one in the list
JointListElement* currentElement = mJointsList;
@ -185,7 +196,8 @@ void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, cons
JointListElement* elementToRemove = currentElement->next;
currentElement->next = elementToRemove->next;
elementToRemove->~JointListElement();
memoryAllocator.release(elementToRemove, sizeof(JointListElement));
memoryManager.release(MemoryManager::AllocationType::Pool,
elementToRemove, sizeof(JointListElement));
break;
}
currentElement = currentElement->next;
@ -214,9 +226,16 @@ ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape,
decimal mass) {
// Create a new proxy collision shape to attach the collision shape to the body
ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate(
ProxyShape* proxyShape = new (mWorld.mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
sizeof(ProxyShape))) ProxyShape(this, collisionShape,
transform, mass);
transform, mass, mWorld.mMemoryManager);
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
proxyShape->setProfiler(mProfiler);
#endif
// Add it to the list of proxy collision shapes of the body
if (mProxyCollisionShapes == nullptr) {
@ -314,6 +333,9 @@ void RigidBody::setTransform(const Transform& transform) {
// Update the linear velocity of the center of mass
mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
// Update the broad-phase state of the body
updateBroadPhaseState();
}
@ -326,6 +348,7 @@ void RigidBody::recomputeMassInformation() {
mMassInverse = decimal(0.0);
mInertiaTensorLocal.setToZero();
mInertiaTensorLocalInverse.setToZero();
mInertiaTensorInverseWorld.setToZero();
mCenterOfMassLocal.setToZero();
// If it is STATIC or KINEMATIC body
@ -386,6 +409,9 @@ void RigidBody::recomputeMassInformation() {
// Compute the local inverse inertia tensor
mInertiaTensorLocalInverse = mInertiaTensorLocal.getInverse();
// Update the world inverse inertia tensor
updateInertiaTensorInverseWorld();
// Update the linear velocity of the center of mass
mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass);
}
@ -393,7 +419,7 @@ void RigidBody::recomputeMassInformation() {
// Update the broad-phase state for this body (because it has moved for instance)
void RigidBody::updateBroadPhaseState() const {
PROFILE("RigidBody::updateBroadPhaseState()");
PROFILE("RigidBody::updateBroadPhaseState()", mProfiler);
DynamicsWorld& world = static_cast<DynamicsWorld&>(mWorld);
const Vector3 displacement = world.mTimeStep * mLinearVelocity;
@ -410,3 +436,22 @@ void RigidBody::updateBroadPhaseState() const {
}
}
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
void RigidBody::setProfiler(Profiler* profiler) {
CollisionBody::setProfiler(profiler);
// Set the profiler for each proxy shape
ProxyShape* proxyShape = getProxyShapesList();
while (proxyShape != nullptr) {
proxyShape->setProfiler(profiler);
proxyShape = proxyShape->getNext();
}
}
#endif

41
src/body/RigidBody.h
View File

@ -31,7 +31,7 @@
#include "CollisionBody.h"
#include "engine/Material.h"
#include "mathematics/mathematics.h"
#include "memory/MemoryAllocator.h"
#include "memory/MemoryManager.h"
/// Namespace reactphysics3d
namespace reactphysics3d {
@ -50,6 +50,11 @@ class DynamicsWorld;
*/
class RigidBody : public CollisionBody {
private :
/// Index of the body in arrays for contact/constraint solver
uint mArrayIndex;
protected :
// -------------------- Attributes -------------------- //
@ -83,6 +88,9 @@ class RigidBody : public CollisionBody {
/// Inverse of the inertia tensor of the body
Matrix3x3 mInertiaTensorLocalInverse;
/// Inverse of the world inertia tensor of the body
Matrix3x3 mInertiaTensorInverseWorld;
/// Inverse of the mass of the body
decimal mMassInverse;
@ -99,12 +107,12 @@ class RigidBody : public CollisionBody {
decimal mAngularDamping;
/// First element of the linked list of joints involving this body
JointListElement* mJointsList;
JointListElement* mJointsList;
// -------------------- Methods -------------------- //
/// Remove a joint from the joints list
void removeJointFromJointsList(MemoryAllocator& memoryAllocator, const Joint* joint);
void removeJointFromJointsList(reactphysics3d::MemoryManager& memoryManager, const Joint* joint);
/// Update the transform of the body after a change of the center of mass
void updateTransformWithCenterOfMass();
@ -112,6 +120,9 @@ class RigidBody : public CollisionBody {
/// Update the broad-phase state for this body (because it has moved for instance)
virtual void updateBroadPhaseState() const override;
/// Update the world inverse inertia tensor of the body
void updateInertiaTensorInverseWorld();
public :
// -------------------- Methods -------------------- //
@ -221,6 +232,13 @@ class RigidBody : public CollisionBody {
/// the collision shapes attached to the body.
void recomputeMassInformation();
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
void setProfiler(Profiler* profiler) override;
#endif
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
@ -291,12 +309,19 @@ inline Matrix3x3 RigidBody::getInertiaTensorWorld() const {
*/
inline Matrix3x3 RigidBody::getInertiaTensorInverseWorld() const {
// TODO : DO NOT RECOMPUTE THE MATRIX MULTIPLICATION EVERY TIME. WE NEED TO STORE THE
// INVERSE WORLD TENSOR IN THE CLASS AND UPLDATE IT WHEN THE ORIENTATION OF THE BODY CHANGES
// Compute and return the inertia tensor in world coordinates
return mTransform.getOrientation().getMatrix() * mInertiaTensorLocalInverse *
mTransform.getOrientation().getMatrix().getTranspose();
return mInertiaTensorInverseWorld;
}
// Update the world inverse inertia tensor of the body
/// The inertia tensor I_w in world coordinates is computed with the
/// local inverse inertia tensor I_b^-1 in body coordinates
/// by I_w = R * I_b^-1 * R^T
/// where R is the rotation matrix (and R^T its transpose) of the
/// current orientation quaternion of the body
inline void RigidBody::updateInertiaTensorInverseWorld() {
Matrix3x3 orientation = mTransform.getOrientation().getMatrix();
mInertiaTensorInverseWorld = orientation * mInertiaTensorLocalInverse * orientation.getTranspose();
}
// Return true if the gravity needs to be applied to this rigid body

82
src/collision/CollisionCallback.cpp Normal file
View File

@ -0,0 +1,82 @@
/********************************************************************************
* 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 "collision/CollisionCallback.h"
#include "engine/OverlappingPair.h"
#include "memory/MemoryAllocator.h"
#include "collision/ContactManifold.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
CollisionCallback::CollisionCallbackInfo::CollisionCallbackInfo(OverlappingPair* pair, MemoryManager& memoryManager) :
contactManifoldElements(nullptr), body1(pair->getShape1()->getBody()),
body2(pair->getShape2()->getBody()),
proxyShape1(pair->getShape1()), proxyShape2(pair->getShape2()),
mMemoryManager(memoryManager) {
assert(pair != nullptr);
const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
// For each contact manifold in the set of manifolds in the pair
ContactManifold* contactManifold = manifoldSet.getContactManifolds();
assert(contactManifold != nullptr);
while (contactManifold != nullptr) {
assert(contactManifold->getNbContactPoints() > 0);
// Add the contact manifold at the beginning of the linked
// list of contact manifolds of the first body
ContactManifoldListElement* element = new (mMemoryManager.allocate(MemoryManager::AllocationType::Pool,
sizeof(ContactManifoldListElement)))
ContactManifoldListElement(contactManifold,
contactManifoldElements);
contactManifoldElements = element;
contactManifold = contactManifold->getNext();
}
}
// Destructor
CollisionCallback::CollisionCallbackInfo::~CollisionCallbackInfo() {
// Release memory allocator for the contact manifold list elements
ContactManifoldListElement* element = contactManifoldElements;
while (element != nullptr) {
ContactManifoldListElement* nextElement = element->getNext();
// Delete and release memory
element->~ContactManifoldListElement();
mMemoryManager.release(MemoryManager::AllocationType::Pool, element,
sizeof(ContactManifoldListElement));
element = nextElement;
}
}

91
src/collision/CollisionCallback.h Normal file
View File

@ -0,0 +1,91 @@
/********************************************************************************
* 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_COLLISION_CALLBACK_H
#define REACTPHYSICS3D_COLLISION_CALLBACK_H
// Libraries
#include "collision/ContactManifold.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
class OverlappingPair;
// Class CollisionCallback
/**
* This class can be used to register a callback for collision test queries.
* You should implement your own class inherited from this one and implement
* the notifyContact() method. This method will called each time a contact
* point is reported.
*/
class CollisionCallback {
public:
// Structure CollisionCallbackInfo
/**
* This structure represents the contact information between two collision
* shapes of two bodies
*/
struct CollisionCallbackInfo {
public:
/// Pointer to the first element of the linked-list that contains contact manifolds
ContactManifoldListElement* contactManifoldElements;
/// Pointer to the first body of the contact
CollisionBody* body1;
/// Pointer to the second body of the contact
CollisionBody* body2;
/// Pointer to the proxy shape of first body
const ProxyShape* proxyShape1;
/// Pointer to the proxy shape of second body
const ProxyShape* proxyShape2;
/// Memory manager
MemoryManager& mMemoryManager;
// Constructor
CollisionCallbackInfo(OverlappingPair* pair, MemoryManager& memoryManager);
// Destructor
~CollisionCallbackInfo();
};
/// Destructor
virtual ~CollisionCallback() = default;
/// This method will be called for each reported contact point
virtual void notifyContact(const CollisionCallbackInfo& collisionCallbackInfo)=0;
};
}
#endif

1121
src/collision/CollisionDetection.cpp
View File

File diff suppressed because it is too large Load Diff

198
src/collision/CollisionDetection.h
View File

@ -32,7 +32,7 @@
#include "engine/OverlappingPair.h"
#include "engine/EventListener.h"
#include "narrowphase/DefaultCollisionDispatch.h"
#include "memory/MemoryAllocator.h"
#include "memory/MemoryManager.h"
#include "constraint/ContactPoint.h"
#include <vector>
#include <set>
@ -46,29 +46,7 @@ namespace reactphysics3d {
class BroadPhaseAlgorithm;
class CollisionWorld;
class CollisionCallback;
// Class TestCollisionBetweenShapesCallback
class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback {
private:
CollisionCallback* mCollisionCallback;
public:
// Constructor
TestCollisionBetweenShapesCallback(CollisionCallback* callback)
: mCollisionCallback(callback) {
}
// 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) override;
};
class OverlapCallback;
// Class CollisionDetection
/**
@ -77,12 +55,15 @@ class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback {
* collide and then we run a narrow-phase algorithm to compute the
* collision contacts between bodies.
*/
class CollisionDetection : public NarrowPhaseCallback {
class CollisionDetection {
private :
// -------------------- Attributes -------------------- //
/// Memory manager
MemoryManager& mMemoryManager;
/// Collision Detection Dispatch configuration
CollisionDispatch* mCollisionDispatch;
@ -92,18 +73,15 @@ class CollisionDetection : public NarrowPhaseCallback {
/// Collision detection matrix (algorithms to use)
NarrowPhaseAlgorithm* mCollisionMatrix[NB_COLLISION_SHAPE_TYPES][NB_COLLISION_SHAPE_TYPES];
/// Reference to the memory allocator
MemoryAllocator& mMemoryAllocator;
/// Pointer to the physics world
CollisionWorld* mWorld;
/// Pointer to the first narrow-phase info of the linked list
NarrowPhaseInfo* mNarrowPhaseInfoList;
/// Broad-phase overlapping pairs
std::map<overlappingpairid, OverlappingPair*> mOverlappingPairs;
/// Overlapping pairs in contact (during the current Narrow-phase collision detection)
std::map<overlappingpairid, OverlappingPair*> mContactOverlappingPairs;
/// Broad-phase algorithm
BroadPhaseAlgorithm mBroadPhaseAlgorithm;
@ -111,17 +89,28 @@ class CollisionDetection : public NarrowPhaseCallback {
// TODO : Delete this
GJKAlgorithm mNarrowPhaseGJKAlgorithm;
// TODO : Maybe delete this set (what is the purpose ?)
/// Set of pair of bodies that cannot collide between each other
std::set<bodyindexpair> mNoCollisionPairs;
/// True if some collision shapes have been added previously
bool mIsCollisionShapesAdded;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
// -------------------- Methods -------------------- //
/// Compute the broad-phase collision detection
void computeBroadPhase();
/// Compute the middle-phase collision detection
void computeMiddlePhase();
/// Compute the narrow-phase collision detection
void computeNarrowPhase();
@ -129,21 +118,41 @@ class CollisionDetection : public NarrowPhaseCallback {
/// involed in the corresponding contact.
void addContactManifoldToBody(OverlappingPair* pair);
/// Delete all the contact points in the currently overlapping pairs
void clearContactPoints();
/// Fill-in the collision detection matrix
void fillInCollisionMatrix();
/// Return the corresponding narrow-phase algorithm
NarrowPhaseAlgorithm* selectNarrowPhaseAlgorithm(const CollisionShapeType& shape1Type,
const CollisionShapeType& shape2Type) const;
/// Add all the contact manifold of colliding pairs to their bodies
void addAllContactManifoldsToBodies();
/// Compute the concave vs convex middle-phase algorithm for a given pair of bodies
void computeConvexVsConcaveMiddlePhase(OverlappingPair* pair, MemoryAllocator& allocator,
NarrowPhaseInfo** firstNarrowPhaseInfo);
/// Compute the middle-phase collision detection between two proxy shapes
NarrowPhaseInfo* computeMiddlePhaseForProxyShapes(OverlappingPair* pair);
/// Convert the potential contact into actual contacts
void processAllPotentialContacts();
/// Process the potential contact manifold of a pair to create actual contact manifold
void processPotentialContacts(OverlappingPair* pair);
/// Report contacts for all the colliding overlapping pairs
void reportAllContacts();
/// Process the potential contacts where one collion is a concave shape
void processSmoothMeshContacts(OverlappingPair* pair);
public :
// -------------------- Methods -------------------- //
/// Constructor
CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator);
CollisionDetection(CollisionWorld* world, MemoryManager& memoryManager);
/// Destructor
~CollisionDetection() = default;
@ -157,10 +166,6 @@ class CollisionDetection : public NarrowPhaseCallback {
/// Set the collision dispatch configuration
void setCollisionDispatch(CollisionDispatch* collisionDispatch);
/// Return the Narrow-phase collision detection algorithm to use between two types of shapes
NarrowPhaseAlgorithm* getCollisionAlgorithm(CollisionShapeType shape1Type,
CollisionShapeType shape2Type) const;
/// Add a proxy collision shape to the collision detection
void addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb);
@ -183,50 +188,46 @@ class CollisionDetection : public NarrowPhaseCallback {
/// Compute the collision detection
void computeCollisionDetection();
/// Compute the collision detection
void testCollisionBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1,
const std::set<uint>& shapes2);
/// Report collision between two sets of shapes
void reportCollisionBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1,
const std::set<uint>& shapes2) ;
/// Ray casting method
void raycast(RaycastCallback* raycastCallback, const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const;
/// Test if the AABBs of two bodies overlap
bool testAABBOverlap(const CollisionBody* body1,
const CollisionBody* body2) const;
/// Report all the bodies that overlap with the aabb in parameter
void testAABBOverlap(const AABB& aabb, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF);
/// Test if the AABBs of two proxy shapes overlap
bool testAABBOverlap(const ProxyShape* shape1,
const ProxyShape* shape2) const;
/// Return true if two bodies overlap
bool testOverlap(CollisionBody* body1, CollisionBody* body2);
/// Report all the bodies that overlap with the body in parameter
void testOverlap(CollisionBody* body, OverlapCallback* overlapCallback, unsigned short categoryMaskBits = 0xFFFF);
/// Test and report collisions between two bodies
void testCollision(CollisionBody* body1, CollisionBody* body2, CollisionCallback* callback);
/// Test and report collisions between a body and all the others bodies of the world
void testCollision(CollisionBody* body, CollisionCallback* callback, unsigned short categoryMaskBits = 0xFFFF);
/// Test and report collisions between all shapes of the world
void testCollision(CollisionCallback* callback);
/// Allow the broadphase to notify the collision detection about an overlapping pair.
void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
/// Compute the narrow-phase collision detection
void computeNarrowPhaseBetweenShapes(CollisionCallback* callback,
const std::set<uint>& shapes1,
const std::set<uint>& shapes2);
/// Return a pointer to the world
CollisionWorld* getWorld();
/// Return the world event listener
EventListener* getWorldEventListener();
/// Return a reference to the world memory allocator
MemoryAllocator& getWorldMemoryAllocator();
#ifdef IS_PROFILING_ACTIVE
/// Called by a narrow-phase collision algorithm when a new contact has been found
virtual void notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) override;
/// Set the profiler
void setProfiler(Profiler* profiler);
/// Create a new contact
void createContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo);
#endif
/// Return the world-space AABB of a given proxy shape
const AABB getWorldAABB(const ProxyShape* proxyShape) const;
// -------------------- Friendship -------------------- //
@ -234,20 +235,20 @@ class CollisionDetection : public NarrowPhaseCallback {
friend class ConvexMeshShape;
};
// Return the Narrow-phase collision detection algorithm to use between two types of shapes
inline NarrowPhaseAlgorithm* CollisionDetection::getCollisionAlgorithm(CollisionShapeType shape1Type,
CollisionShapeType shape2Type) const {
return mCollisionMatrix[static_cast<int>(shape1Type)][static_cast<int>(shape2Type)];
}
// Set the collision dispatch configuration
inline void CollisionDetection::setCollisionDispatch(CollisionDispatch* collisionDispatch) {
mCollisionDispatch = collisionDispatch;
mCollisionDispatch->init(this, &mMemoryAllocator);
// Fill-in the collision matrix with the new algorithms to use
fillInCollisionMatrix();
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
mCollisionDispatch->setProfiler(mProfiler);
#endif
}
// Add a body to the collision detection
@ -276,7 +277,10 @@ inline void CollisionDetection::removeNoCollisionPair(CollisionBody* body1,
/// We simply put the shape in the list of collision shape that have moved in the
/// previous frame so that it is tested for collision again in the broad-phase.
inline void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape) {
mBroadPhaseAlgorithm.addMovedCollisionShape(shape->mBroadPhaseID);
if (shape->mBroadPhaseID != -1) {
mBroadPhaseAlgorithm.addMovedCollisionShape(shape->mBroadPhaseID);
}
}
// Update a proxy collision shape (that has moved for instance)
@ -285,12 +289,31 @@ inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, con
mBroadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement);
}
// Return the corresponding narrow-phase algorithm
inline NarrowPhaseAlgorithm* CollisionDetection::selectNarrowPhaseAlgorithm(const CollisionShapeType& shape1Type,
const CollisionShapeType& shape2Type) const {
uint shape1Index = static_cast<unsigned int>(shape1Type);
uint shape2Index = static_cast<unsigned int>(shape2Type);
// Swap the shape types if necessary
if (shape1Index > shape2Index) {
const uint tempIndex = shape1Index;
shape1Index = shape2Index;
shape2Index = tempIndex;
}
assert(shape1Index <= shape2Index);
return mCollisionMatrix[shape1Index][shape2Index];
}
// Ray casting method
inline void CollisionDetection::raycast(RaycastCallback* raycastCallback,
const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const {
PROFILE("CollisionDetection::raycast()");
PROFILE("CollisionDetection::raycast()", mProfiler);
RaycastTest rayCastTest(raycastCallback);
@ -299,23 +322,22 @@ inline void CollisionDetection::raycast(RaycastCallback* raycastCallback,
mBroadPhaseAlgorithm.raycast(ray, rayCastTest, raycastWithCategoryMaskBits);
}
// Test if the AABBs of two proxy shapes overlap
inline bool CollisionDetection::testAABBOverlap(const ProxyShape* shape1,
const ProxyShape* shape2) const {
// If one of the shape's body is not active, we return no overlap
if (!shape1->getBody()->isActive() || !shape2->getBody()->isActive()) {
return false;
}
return mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2);
}
// Return a pointer to the world
inline CollisionWorld* CollisionDetection::getWorld() {
return mWorld;
}
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void CollisionDetection::setProfiler(Profiler* profiler) {
mProfiler = profiler;
mBroadPhaseAlgorithm.setProfiler(profiler);
mCollisionDispatch->setProfiler(profiler);
}
#endif
}
#endif

347
src/collision/ContactManifold.cpp
View File

@ -30,234 +30,159 @@
using namespace reactphysics3d;
// Constructor
ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, short normalDirectionId)
: mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId),
ContactManifold::ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& memoryAllocator)
: mShape1(shape1), mShape2(shape2), mContactPoints(nullptr),
mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
mMemoryAllocator(memoryAllocator) {
mMemoryAllocator(memoryAllocator), mNext(nullptr), mPrevious(nullptr), mIsObsolete(false) {
// For each contact point info in the manifold
const ContactPointInfo* pointInfo = manifoldInfo->getFirstContactPointInfo();
while(pointInfo != nullptr) {
// Add the new contact point
addContactPoint(pointInfo);
pointInfo = pointInfo->next;
}
assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD);
assert(mNbContactPoints > 0);
}
// Destructor
ContactManifold::~ContactManifold() {
clear();
// Delete all the contact points
ContactPoint* contactPoint = mContactPoints;
while(contactPoint != nullptr) {
ContactPoint* nextContactPoint = contactPoint->getNext();
// Delete the contact point
contactPoint->~ContactPoint();
mMemoryAllocator.release(contactPoint, sizeof(ContactPoint));
contactPoint = nextContactPoint;
}
}
// Add a contact point in the manifold
void ContactManifold::addContactPoint(ContactPoint* contact) {
// For contact already in the manifold
for (uint i=0; i<mNbContactPoints; i++) {
// Check if the new point point does not correspond to a same contact point
// already in the manifold.
decimal distance = (mContactPoints[i]->getWorldPointOnBody1() -
contact->getWorldPointOnBody1()).lengthSquare();
if (distance <= PERSISTENT_CONTACT_DIST_THRESHOLD*PERSISTENT_CONTACT_DIST_THRESHOLD) {
// Delete the new contact
contact->~ContactPoint();
mMemoryAllocator.release(contact, sizeof(ContactPoint));
assert(mNbContactPoints > 0);
return;
}
}
// If the contact manifold is full
if (mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD) {
int indexMaxPenetration = getIndexOfDeepestPenetration(contact);
int indexToRemove = getIndexToRemove(indexMaxPenetration, contact->getLocalPointOnBody1());
removeContactPoint(indexToRemove);
}
// Add the new contact point in the manifold
mContactPoints[mNbContactPoints] = contact;
mNbContactPoints++;
// Remove a contact point
void ContactManifold::removeContactPoint(ContactPoint* contactPoint) {
assert(mNbContactPoints > 0);
}
assert(mContactPoints != nullptr);
assert(contactPoint != nullptr);
// Remove a contact point from the manifold
void ContactManifold::removeContactPoint(uint index) {
assert(index < mNbContactPoints);
assert(mNbContactPoints > 0);
// Call the destructor explicitly and tell the memory allocator that
// the corresponding memory block is now free
mContactPoints[index]->~ContactPoint();
mMemoryAllocator.release(mContactPoints[index], sizeof(ContactPoint));
// If we don't remove the last index
if (index < mNbContactPoints - 1) {
mContactPoints[index] = mContactPoints[mNbContactPoints - 1];
}
ContactPoint* previous = contactPoint->getPrevious();
ContactPoint* next = contactPoint->getNext();
mNbContactPoints--;
}
// Update the contact manifold
/// First the world space coordinates of the current contacts in the manifold are recomputed from
/// the corresponding transforms of the bodies because they have moved. Then we remove the contacts
/// with a negative penetration depth (meaning that the bodies are not penetrating anymore) and also
/// the contacts with a too large distance between the contact points in the plane orthogonal to the
/// contact normal.
void ContactManifold::update(const Transform& transform1, const Transform& transform2) {
if (mNbContactPoints == 0) return;
// Update the world coordinates and penetration depth of the contact points in the manifold
for (uint i=0; i<mNbContactPoints; i++) {
mContactPoints[i]->setWorldPointOnBody1(transform1 *
mContactPoints[i]->getLocalPointOnBody1());
mContactPoints[i]->setWorldPointOnBody2(transform2 *
mContactPoints[i]->getLocalPointOnBody2());
mContactPoints[i]->setPenetrationDepth((mContactPoints[i]->getWorldPointOnBody1() -
mContactPoints[i]->getWorldPointOnBody2()).dot(mContactPoints[i]->getNormal()));
}
const decimal squarePersistentContactThreshold = PERSISTENT_CONTACT_DIST_THRESHOLD *
PERSISTENT_CONTACT_DIST_THRESHOLD;
// Remove the contact points that don't represent very well the contact manifold
for (int i=static_cast<int>(mNbContactPoints)-1; i>=0; i--) {
assert(i < static_cast<int>(mNbContactPoints));
// Compute the distance between contact points in the normal direction
decimal distanceNormal = -mContactPoints[i]->getPenetrationDepth();
// If the contacts points are too far from each other in the normal direction
if (distanceNormal > squarePersistentContactThreshold) {
removeContactPoint(i);
}
else {
// Compute the distance of the two contact points in the plane
// orthogonal to the contact normal
Vector3 projOfPoint1 = mContactPoints[i]->getWorldPointOnBody1() +
mContactPoints[i]->getNormal() * distanceNormal;
Vector3 projDifference = mContactPoints[i]->getWorldPointOnBody2() - projOfPoint1;
// If the orthogonal distance is larger than the valid distance
// threshold, we remove the contact
if (projDifference.lengthSquare() > squarePersistentContactThreshold) {
removeContactPoint(i);
}
}
}
}
// Return the index of the contact point with the larger penetration depth.
/// This corresponding contact will be kept in the cache. The method returns -1 is
/// the new contact is the deepest.
int ContactManifold::getIndexOfDeepestPenetration(ContactPoint* newContact) const {
assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
int indexMaxPenetrationDepth = -1;
decimal maxPenetrationDepth = newContact->getPenetrationDepth();
// For each contact in the cache
for (uint i=0; i<mNbContactPoints; i++) {
// If the current contact has a larger penetration depth
if (mContactPoints[i]->getPenetrationDepth() > maxPenetrationDepth) {
maxPenetrationDepth = mContactPoints[i]->getPenetrationDepth();
indexMaxPenetrationDepth = i;
}
}
// Return the index of largest penetration depth
return indexMaxPenetrationDepth;
}
// Return the index that will be removed.
/// The index of the contact point with the larger penetration
/// depth is given as a parameter. This contact won't be removed. Given this contact, we compute
/// the different area and we want to keep the contacts with the largest area. The new point is also
/// kept. In order to compute the area of a quadrilateral, we use the formula :
/// Area = 0.5 * | AC x BD | where AC and BD form the diagonals of the quadrilateral. Note that
/// when we compute this area, we do not calculate it exactly but we
/// only estimate it because we do not compute the actual diagonals of the quadrialteral. Therefore,
/// this is only a guess that is faster to compute. This idea comes from the Bullet Physics library
/// by Erwin Coumans (http://wwww.bulletphysics.org).
int ContactManifold::getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const {
assert(mNbContactPoints == MAX_CONTACT_POINTS_IN_MANIFOLD);
decimal area0 = 0.0; // Area with contact 1,2,3 and newPoint
decimal area1 = 0.0; // Area with contact 0,2,3 and newPoint
decimal area2 = 0.0; // Area with contact 0,1,3 and newPoint
decimal area3 = 0.0; // Area with contact 0,1,2 and newPoint
if (indexMaxPenetration != 0) {
// Compute the area
Vector3 vector1 = newPoint - mContactPoints[1]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
mContactPoints[2]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area0 = crossProduct.lengthSquare();
}
if (indexMaxPenetration != 1) {
// Compute the area
Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
mContactPoints[2]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area1 = crossProduct.lengthSquare();
}
if (indexMaxPenetration != 2) {
// Compute the area
Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[3]->getLocalPointOnBody1() -
mContactPoints[1]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area2 = crossProduct.lengthSquare();
}
if (indexMaxPenetration != 3) {
// Compute the area
Vector3 vector1 = newPoint - mContactPoints[0]->getLocalPointOnBody1();
Vector3 vector2 = mContactPoints[2]->getLocalPointOnBody1() -
mContactPoints[1]->getLocalPointOnBody1();
Vector3 crossProduct = vector1.cross(vector2);
area3 = crossProduct.lengthSquare();
}
// Return the index of the contact to remove
return getMaxArea(area0, area1, area2, area3);
}
// Return the index of maximum area
int ContactManifold::getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const {
if (area0 < area1) {
if (area1 < area2) {
if (area2 < area3) return 3;
else return 2;
}
else {
if (area1 < area3) return 3;
else return 1;
}
if (previous != nullptr) {
previous->setNext(next);
}
else {
if (area0 < area2) {
if (area2 < area3) return 3;
else return 2;
}
else {
if (area0 < area3) return 3;
else return 0;
}
mContactPoints = next;
}
if (next != nullptr) {
next->setPrevious(previous);
}
// Delete the contact point
contactPoint->~ContactPoint();
mMemoryAllocator.release(contactPoint, sizeof(ContactPoint));
mNbContactPoints--;
assert(mNbContactPoints >= 0);
}
// Clear the contact manifold
void ContactManifold::clear() {
for (uint i=0; i<mNbContactPoints; i++) {
// Call the destructor explicitly and tell the memory allocator that
// the corresponding memory block is now free
mContactPoints[i]->~ContactPoint();
mMemoryAllocator.release(mContactPoints[i], sizeof(ContactPoint));
// Add a contact point
void ContactManifold::addContactPoint(const ContactPointInfo* contactPointInfo) {
assert(contactPointInfo != nullptr);
// Create the new contact point
ContactPoint* contactPoint = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint(contactPointInfo);
// Add the new contact point into the manifold
contactPoint->setNext(mContactPoints);
contactPoint->setPrevious(nullptr);
if (mContactPoints != nullptr) {
mContactPoints->setPrevious(contactPoint);
}
mNbContactPoints = 0;
mContactPoints = contactPoint;
mNbContactPoints++;
}
// Clear the obsolete contact points
void ContactManifold::clearObsoleteContactPoints() {
assert(mContactPoints != nullptr);
// For each contact point of the manifold
ContactPoint* contactPoint = mContactPoints;
while (contactPoint != nullptr) {
ContactPoint* nextContactPoint = contactPoint->getNext();
// If the contact point is obsolete
if (contactPoint->getIsObsolete()) {
// Remove the contact point
removeContactPoint(contactPoint);
}
contactPoint = nextContactPoint;
}
assert(mNbContactPoints > 0);
assert(mContactPoints != nullptr);
}
// Make sure we do not have too much contact points by keeping only the best
// contact points of the manifold (with largest penetration depth)
void ContactManifold::reduce() {
assert(mContactPoints != nullptr);
// Remove contact points while there is too much contact points
while (mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) {
removeNonOptimalContactPoint();
}
assert(mNbContactPoints <= MAX_CONTACT_POINTS_IN_MANIFOLD && mNbContactPoints > 0);
assert(mContactPoints != nullptr);
}
// Remove a contact point that is not optimal (with a small penetration depth)
void ContactManifold::removeNonOptimalContactPoint() {
assert(mContactPoints != nullptr);
assert(mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD);
// Get the contact point with the minimum penetration depth among all points
ContactPoint* contactPoint = mContactPoints;
ContactPoint* minContactPoint = nullptr;
decimal minPenetrationDepth = DECIMAL_LARGEST;
while (contactPoint != nullptr) {
ContactPoint* nextContactPoint = contactPoint->getNext();
if (contactPoint->getPenetrationDepth() < minPenetrationDepth) {
minContactPoint = contactPoint;
minPenetrationDepth = contactPoint->getPenetrationDepth();
}
contactPoint = nextContactPoint;
}
assert(minContactPoint != nullptr);
// Remove the non optimal contact point
removeContactPoint(minContactPoint);
assert(mNbContactPoints > 0);
assert(mContactPoints != nullptr);
}

285
src/collision/ContactManifold.h
View File

@ -31,14 +31,12 @@
#include "body/CollisionBody.h"
#include "collision/ProxyShape.h"
#include "constraint/ContactPoint.h"
#include "memory/MemoryAllocator.h"
#include "collision/ContactManifoldInfo.h"
#include "memory/PoolAllocator.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Constants
const uint MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold
// Class declarations
class ContactManifold;
@ -48,40 +46,48 @@ class ContactManifold;
*/
struct ContactManifoldListElement {
public:
private:
// -------------------- Attributes -------------------- //
/// Pointer to the actual contact manifold
ContactManifold* contactManifold;
/// Pointer to a contact manifold with contact points
ContactManifold* mContactManifold;
/// Next element of the list
ContactManifoldListElement* next;
ContactManifoldListElement* mNext;
public:
// -------------------- Methods -------------------- //
/// Constructor
ContactManifoldListElement(ContactManifold* initContactManifold,
ContactManifoldListElement* initNext)
:contactManifold(initContactManifold), next(initNext) {
ContactManifoldListElement(ContactManifold* contactManifold,
ContactManifoldListElement* next)
:mContactManifold(contactManifold), mNext(next) {
}
/// Return the contact manifold
ContactManifold* getContactManifold() {
return mContactManifold;
}
/// Return the next element in the linked-list
ContactManifoldListElement* getNext() {
return mNext;
}
};
// Class ContactManifold
/**
* This class represents the set of contact points between two bodies.
* This class represents a set of contact points between two bodies that
* all have a similar contact normal direction. Usually, there is a single
* contact manifold when two convex shapes are in contact. However, when
* a convex shape collides with a concave shape, there can be several
* contact manifolds with different normal directions.
* The contact manifold is implemented in a way to cache the contact
* points among the frames for better stability following the
* "Contact Generation" presentation of Erwin Coumans at GDC 2010
* conference (bullet.googlecode.com/files/GDC10_Coumans_Erwin_Contact.pdf).
* Some code of this class is based on the implementation of the
* btPersistentManifold class from Bullet physics engine (www.http://bulletphysics.org).
* The contacts between two bodies are added one after the other in the cache.
* When the cache is full, we have to remove one point. The idea is to keep
* the point with the deepest penetration depth and also to keep the
* points producing the larger area (for a more stable contact manifold).
* The new added point is always kept.
* points among the frames for better stability (warm starting of the
* contact solver)
*/
class ContactManifold {
@ -96,13 +102,10 @@ class ContactManifold {
ProxyShape* mShape2;
/// Contact points in the manifold
ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD];
/// Normal direction Id (Unique Id representing the normal direction)
short int mNormalDirectionId;
ContactPoint* mContactPoints;
/// Number of contacts in the cache
uint mNbContactPoints;
int8 mNbContactPoints;
/// First friction vector of the contact manifold
Vector3 mFrictionVector1;
@ -128,30 +131,93 @@ class ContactManifold {
/// Reference to the memory allocator
MemoryAllocator& mMemoryAllocator;
/// Pointer to the next contact manifold in the linked-list
ContactManifold* mNext;
/// Pointer to the previous contact manifold in linked-list
ContactManifold* mPrevious;
/// True if the contact manifold is obsolete
bool mIsObsolete;
// -------------------- Methods -------------------- //
/// Return the index of maximum area
int getMaxArea(decimal area0, decimal area1, decimal area2, decimal area3) const;
/// Return the index of the contact with the larger penetration depth.
int getIndexOfDeepestPenetration(ContactPoint* newContact) const;
/// Return the index that will be removed.
int getIndexToRemove(int indexMaxPenetration, const Vector3& newPoint) const;
/// Remove a contact point from the manifold
void removeContactPoint(uint index);
/// Return true if the contact manifold has already been added into an island
bool isAlreadyInIsland() const;
/// Set the pointer to the next element in the linked-list
void setNext(ContactManifold* nextManifold);
/// Return true if the manifold is obsolete
bool getIsObsolete() const;
/// Set to true to make the manifold obsolete
void setIsObsolete(bool isObselete, bool setContactPoints);
/// Clear the obsolete contact points
void clearObsoleteContactPoints();
/// Return the contact normal direction Id of the manifold
short getContactNormalId() const;
/// Return the largest depth of all the contact points
decimal getLargestContactDepth() const;
/// set the first friction vector at the center of the contact manifold
void setFrictionVector1(const Vector3& mFrictionVector1);
/// set the second friction vector at the center of the contact manifold
void setFrictionVector2(const Vector3& mFrictionVector2);
/// Set the first friction accumulated impulse
void setFrictionImpulse1(decimal frictionImpulse1);
/// Set the second friction accumulated impulse
void setFrictionImpulse2(decimal frictionImpulse2);
/// Add a contact point
void addContactPoint(const ContactPointInfo* contactPointInfo);
/// Make sure we do not have too much contact points by keeping only the best ones
void reduce();
/// Remove a contact point that is not optimal (with a small penetration depth)
void removeNonOptimalContactPoint();
/// Remove a contact point
void removeContactPoint(ContactPoint* contactPoint);
/// Set the friction twist accumulated impulse
void setFrictionTwistImpulse(decimal frictionTwistImpulse);
/// Set the accumulated rolling resistance impulse
void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse);
/// Set the pointer to the previous element in the linked-list
void setPrevious(ContactManifold* previousManifold);
/// Return the first friction vector at the center of the contact manifold
const Vector3& getFrictionVector1() const;
/// Return the second friction vector at the center of the contact manifold
const Vector3& getFrictionVector2() const;
/// Return the first friction accumulated impulse
decimal getFrictionImpulse1() const;
/// Return the second friction accumulated impulse
decimal getFrictionImpulse2() const;
/// Return the friction twist accumulated impulse
decimal getFrictionTwistImpulse() const;
public:
// -------------------- Methods -------------------- //
/// Constructor
ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, short int normalDirectionId);
ContactManifold(const ContactManifoldInfo* manifoldInfo, ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator);
/// Destructor
~ContactManifold();
@ -174,68 +240,25 @@ class ContactManifold {
/// Return a pointer to the second body of the contact manifold
CollisionBody* getBody2() const;
/// Return the normal direction Id
short int getNormalDirectionId() const;
/// Add a contact point to the manifold
void addContactPoint(ContactPoint* contact);
/// Update the contact manifold.
void update(const Transform& transform1, const Transform& transform2);
/// Clear the contact manifold
void clear();
/// Return the number of contact points in the manifold
uint getNbContactPoints() const;
int8 getNbContactPoints() const;
/// Return the first friction vector at the center of the contact manifold
const Vector3& getFrictionVector1() const;
/// Return a pointer to the first contact point of the manifold
ContactPoint* getContactPoints() const;
/// set the first friction vector at the center of the contact manifold
void setFrictionVector1(const Vector3& mFrictionVector1);
/// Return a pointer to the previous element in the linked-list
ContactManifold* getPrevious() const;
/// Return the second friction vector at the center of the contact manifold
const Vector3& getFrictionVector2() const;
/// set the second friction vector at the center of the contact manifold
void setFrictionVector2(const Vector3& mFrictionVector2);
/// Return the first friction accumulated impulse
decimal getFrictionImpulse1() const;
/// Set the first friction accumulated impulse
void setFrictionImpulse1(decimal frictionImpulse1);
/// Return the second friction accumulated impulse
decimal getFrictionImpulse2() const;
/// Set the second friction accumulated impulse
void setFrictionImpulse2(decimal frictionImpulse2);
/// Return the friction twist accumulated impulse
decimal getFrictionTwistImpulse() const;
/// Set the friction twist accumulated impulse
void setFrictionTwistImpulse(decimal frictionTwistImpulse);
/// Set the accumulated rolling resistance impulse
void setRollingResistanceImpulse(const Vector3& rollingResistanceImpulse);
/// Return a contact point of the manifold
ContactPoint* getContactPoint(uint index) const;
/// Return the normalized averaged normal vector
Vector3 getAverageContactNormal() const;
/// Return the largest depth of all the contact points
decimal getLargestContactDepth() const;
/// Return a pointer to the next element in the linked-list
ContactManifold* getNext() const;
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
friend class Island;
friend class CollisionBody;
friend class ContactManifoldSet;
friend class ContactSolver;
};
// Return a pointer to the first proxy shape of the contact
@ -258,13 +281,8 @@ inline CollisionBody* ContactManifold::getBody2() const {
return mShape2->getBody();
}
// Return the normal direction Id
inline short int ContactManifold::getNormalDirectionId() const {
return mNormalDirectionId;
}
// Return the number of contact points in the manifold
inline uint ContactManifold::getNbContactPoints() const {
inline int8 ContactManifold::getNbContactPoints() const {
return mNbContactPoints;
}
@ -323,10 +341,9 @@ inline void ContactManifold::setRollingResistanceImpulse(const Vector3& rollingR
mRollingResistanceImpulse = rollingResistanceImpulse;
}
// Return a contact point of the manifold
inline ContactPoint* ContactManifold::getContactPoint(uint index) const {
assert(index < mNbContactPoints);
return mContactPoints[index];
// Return a pointer to the first contact point of the manifold
inline ContactPoint* ContactManifold::getContactPoints() const {
return mContactPoints;
}
// Return true if the contact manifold has already been added into an island
@ -334,31 +351,65 @@ inline bool ContactManifold::isAlreadyInIsland() const {
return mIsAlreadyInIsland;
}
// Return the normalized averaged normal vector
inline Vector3 ContactManifold::getAverageContactNormal() const {
Vector3 averageNormal;
for (uint i=0; i<mNbContactPoints; i++) {
averageNormal += mContactPoints[i]->getNormal();
}
return averageNormal.getUnit();
}
// Return the largest depth of all the contact points
inline decimal ContactManifold::getLargestContactDepth() const {
decimal largestDepth = 0.0f;
for (uint i=0; i<mNbContactPoints; i++) {
decimal depth = mContactPoints[i]->getPenetrationDepth();
assert(mNbContactPoints > 0);
ContactPoint* contactPoint = mContactPoints;
while(contactPoint != nullptr){
decimal depth = contactPoint->getPenetrationDepth();
if (depth > largestDepth) {
largestDepth = depth;
}
contactPoint = contactPoint->getNext();
}
return largestDepth;
}
// Return a pointer to the previous element in the linked-list
inline ContactManifold* ContactManifold::getPrevious() const {
return mPrevious;
}
// Set the pointer to the previous element in the linked-list
inline void ContactManifold::setPrevious(ContactManifold* previousManifold) {
mPrevious = previousManifold;
}
// Return a pointer to the next element in the linked-list
inline ContactManifold* ContactManifold::getNext() const {
return mNext;
}
// Set the pointer to the next element in the linked-list
inline void ContactManifold::setNext(ContactManifold* nextManifold) {
mNext = nextManifold;
}
// Return true if the manifold is obsolete
inline bool ContactManifold::getIsObsolete() const {
return mIsObsolete;
}
// Set to true to make the manifold obsolete
inline void ContactManifold::setIsObsolete(bool isObsolete, bool setContactPoints) {
mIsObsolete = isObsolete;
if (setContactPoints) {
ContactPoint* contactPoint = mContactPoints;
while (contactPoint != nullptr) {
contactPoint->setIsObsolete(isObsolete);
contactPoint = contactPoint->getNext();
}
}
}
}
#endif

280
src/collision/ContactManifoldInfo.cpp Normal file
View File

@ -0,0 +1,280 @@
/********************************************************************************
* 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 "ContactManifoldInfo.h"
using namespace reactphysics3d;
// Constructor
ContactManifoldInfo::ContactManifoldInfo(MemoryAllocator& allocator)
: mContactPointsList(nullptr), mNbContactPoints(0), mNext(nullptr), mAllocator(allocator) {
}
// Destructor
ContactManifoldInfo::~ContactManifoldInfo() {
// Remove all the contact points
reset();
}
// Add a new contact point into the manifold
void ContactManifoldInfo::addContactPoint(ContactPointInfo* contactPointInfo) {
assert(contactPointInfo->penetrationDepth > decimal(0.0));
// Add it into the linked list of contact points
contactPointInfo->next = mContactPointsList;
mContactPointsList = contactPointInfo;
mNbContactPoints++;
}
// Remove all the contact points
void ContactManifoldInfo::reset() {
// Delete all the contact points in the linked list
ContactPointInfo* element = mContactPointsList;
while(element != nullptr) {
ContactPointInfo* elementToDelete = element;
element = element->next;
// Call the constructor
elementToDelete->~ContactPointInfo();
// Delete the current element
mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
}
mContactPointsList = nullptr;
mNbContactPoints = 0;
}
// Return the largest penetration depth among its contact points
decimal ContactManifoldInfo::getLargestPenetrationDepth() const {
ContactPointInfo* contactPoint = mContactPointsList;
assert(contactPoint != nullptr);
decimal maxDepth = decimal(0.0);
while (contactPoint != nullptr) {
if (contactPoint->penetrationDepth > maxDepth) {
maxDepth = contactPoint->penetrationDepth;
}
contactPoint = contactPoint->next;
}
return maxDepth;
}
// Reduce the number of contact points of the currently computed manifold
// This is based on the technique described by Dirk Gregorius in his
// "Contacts Creation" GDC presentation
void ContactManifoldInfo::reduce(const Transform& shape1ToWorldTransform) {
assert(mContactPointsList != nullptr);
// The following algorithm only works to reduce to 4 contact points
assert(MAX_CONTACT_POINTS_IN_MANIFOLD == 4);
// If there are too many contact points in the manifold
if (mNbContactPoints > MAX_CONTACT_POINTS_IN_MANIFOLD) {
ContactPointInfo* pointsToKeep[MAX_CONTACT_POINTS_IN_MANIFOLD];
// Compute the initial contact point we need to keep.
// The first point we keep is always the point in a given
// constant direction (in order to always have same contact points
// between frames for better stability)
const Transform worldToShape1Transform = shape1ToWorldTransform.getInverse();
// Compute the contact normal of the manifold (we use the first contact point)
// in the local-space of the first collision shape
const Vector3 contactNormalShape1Space = worldToShape1Transform.getOrientation() * mContactPointsList->normal;
// Compute a search direction
const Vector3 searchDirection(1, 1, 1);
ContactPointInfo* element = mContactPointsList;
pointsToKeep[0] = element;
decimal maxDotProduct = searchDirection.dot(element->localPoint1);
element = element->next;
while(element != nullptr) {
decimal dotProduct = searchDirection.dot(element->localPoint1);
if (dotProduct > maxDotProduct) {
maxDotProduct = dotProduct;
pointsToKeep[0] = element;
}
element = element->next;
}
// Compute the second contact point we need to keep.
// The second point we keep is the one farthest away from the first point.
decimal maxDistance = decimal(0.0);
element = mContactPointsList;
while(element != nullptr) {
if (element == pointsToKeep[0]) {
element = element->next;
continue;
}
decimal distance = (pointsToKeep[0]->localPoint1 - element->localPoint1).lengthSquare();
if (distance >= maxDistance) {
maxDistance = distance;
pointsToKeep[1] = element;
}
element = element->next;
}
assert(pointsToKeep[1] != nullptr);
// Compute the third contact point we need to keep.
// The second point is the one producing the triangle with the larger area
// with first and second point.
// We compute the most positive or most negative triangle area (depending on winding)
ContactPointInfo* thirdPointMaxArea = nullptr;
ContactPointInfo* thirdPointMinArea = nullptr;
decimal minArea = decimal(0.0);
decimal maxArea = decimal(0.0);
bool isPreviousAreaPositive = true;
element = mContactPointsList;
while(element != nullptr) {
if (element == pointsToKeep[0] || element == pointsToKeep[1]) {
element = element->next;
continue;
}
const Vector3 newToFirst = pointsToKeep[0]->localPoint1 - element->localPoint1;
const Vector3 newToSecond = pointsToKeep[1]->localPoint1 - element->localPoint1;
// Compute the triangle area
decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space);
if (area >= maxArea) {
maxArea = area;
thirdPointMaxArea = element;
}
if (area <= minArea) {
minArea = area;
thirdPointMinArea = element;
}
element = element->next;
}
assert(minArea <= decimal(0.0));
assert(maxArea >= decimal(0.0));
if (maxArea > (-minArea)) {
isPreviousAreaPositive = true;
pointsToKeep[2] = thirdPointMaxArea;
}
else {
isPreviousAreaPositive = false;
pointsToKeep[2] = thirdPointMinArea;
}
assert(pointsToKeep[2] != nullptr);
// Compute the 4th point by choosing the triangle that add the most
// triangle area to the previous triangle and has opposite sign area (opposite winding)
decimal largestArea = decimal(0.0); // Largest area (positive or negative)
element = mContactPointsList;
// For each remaining point
while(element != nullptr) {
if (element == pointsToKeep[0] || element == pointsToKeep[1] || element == pointsToKeep[2]) {
element = element->next;
continue;
}
// For each edge of the triangle made by the first three points
for (uint i=0; i<3; i++) {
uint edgeVertex1Index = i;
uint edgeVertex2Index = i < 2 ? i + 1 : 0;
const Vector3 newToFirst = pointsToKeep[edgeVertex1Index]->localPoint1 - element->localPoint1;
const Vector3 newToSecond = pointsToKeep[edgeVertex2Index]->localPoint1 - element->localPoint1;
// Compute the triangle area
decimal area = newToFirst.cross(newToSecond).dot(contactNormalShape1Space);
// We are looking at the triangle with maximal area (positive or negative).
// If the previous area is positive, we are looking at negative area now.
// If the previous area is negative, we are looking at the positive area now.
if (isPreviousAreaPositive && area < largestArea) {
largestArea = area;
pointsToKeep[3] = element;
}
else if (!isPreviousAreaPositive && area > largestArea) {
largestArea = area;
pointsToKeep[3] = element;
}
}
element = element->next;
}
assert(pointsToKeep[3] != nullptr);
// Delete the contact points we do not want to keep from the linked list
element = mContactPointsList;
ContactPointInfo* previousElement = nullptr;
while(element != nullptr) {
// Skip the points we want to keep
if (element == pointsToKeep[0] || element == pointsToKeep[1] ||
element == pointsToKeep[2] || element == pointsToKeep[3]) {
previousElement = element;
element = element->next;
continue;
}
ContactPointInfo* elementToDelete = element;
if (previousElement != nullptr) {
previousElement->next = elementToDelete->next;
}
else {
mContactPointsList = elementToDelete->next;
}
element = element->next;
// Call the destructor
elementToDelete->~ContactPointInfo();
// Delete the current element
mAllocator.release(elementToDelete, sizeof(ContactPointInfo));
}
mNbContactPoints = 4;
}
}

122
src/collision/narrowphase/EPA/TrianglesStore.h → src/collision/ContactManifoldInfo.h
View File

@ -23,117 +23,91 @@
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_TRIANGLES_STORE_H
#define REACTPHYSICS3D_TRIANGLES_STORE_H
#include "TriangleEPA.h"
#ifndef REACTPHYSICS3D_CONTACT_MANIFOLD_INFO_H
#define REACTPHYSICS3D_CONTACT_MANIFOLD_INFO_H
// Libraries
#include <cassert>
#include "collision/ContactPointInfo.h"
#include "memory/MemoryAllocator.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Constants
constexpr unsigned int MAX_TRIANGLES = 200; // Maximum number of triangles
const int8 MAX_CONTACT_POINTS_IN_MANIFOLD = 4; // Maximum number of contacts in the manifold
// Class TriangleStore
// Class ContactManifoldInfo
/**
* This class stores several triangles of the polytope in the EPA algorithm.
* This class is used to collect the list of ContactPointInfo that come
* from a collision test between two shapes.
*/
class TrianglesStore {
class ContactManifoldInfo {
private:
// -------------------- Attributes -------------------- //
/// Triangles
TriangleEPA mTriangles[MAX_TRIANGLES];
/// Linked list with all the contact points
ContactPointInfo* mContactPointsList;
/// Number of contact points in the manifold
uint mNbContactPoints;
/// Next element in the linked-list of contact manifold info
ContactManifoldInfo* mNext;
/// Reference the the memory allocator where the contact point infos have been allocated
MemoryAllocator& mAllocator;
/// Number of triangles
int mNbTriangles;
public:
// -------------------- Methods -------------------- //
/// Constructor
TrianglesStore();
ContactManifoldInfo(MemoryAllocator& allocator);
/// Destructor
~TrianglesStore() = default;
~ContactManifoldInfo();
/// Deleted copy-constructor
TrianglesStore(const TrianglesStore& triangleStore) = delete;
/// Deleted Copy-constructor
ContactManifoldInfo(const ContactManifoldInfo& contactManifold) = delete;
/// Deleted assignment operator
TrianglesStore& operator=(const TrianglesStore& triangleStore) = delete;
ContactManifoldInfo& operator=(const ContactManifoldInfo& contactManifold) = delete;
/// Clear all the storage
void clear();
/// Add a new contact point into the manifold
void addContactPoint(ContactPointInfo* contactPointInfo);
/// Return the number of triangles
int getNbTriangles() const;
/// Remove all the contact points
void reset();
/// Set the number of triangles
void setNbTriangles(int backup);
/// Get the first contact point info of the linked list of contact points
ContactPointInfo* getFirstContactPointInfo() const;
/// Return the last triangle
TriangleEPA& last();
/// Return the largest penetration depth among its contact points
decimal getLargestPenetrationDepth() const;
/// Create a new triangle
TriangleEPA* newTriangle(const Vector3* vertices, uint v0, uint v1, uint v2);
/// Return the pointer to the next manifold info in the linked-list
ContactManifoldInfo* getNext();
/// Access operator
TriangleEPA& operator[](int i);
/// Reduce the number of contact points of the currently computed manifold
void reduce(const Transform& shape1ToWorldTransform);
// Friendship
friend class OverlappingPair;
friend class CollisionDetection;
};
// Clear all the storage
inline void TrianglesStore::clear() {
mNbTriangles = 0;
// Get the first contact point info of the linked list of contact points
inline ContactPointInfo* ContactManifoldInfo::getFirstContactPointInfo() const {
return mContactPointsList;
}
// Return the number of triangles
inline int TrianglesStore::getNbTriangles() const {
return mNbTriangles;
}
inline void TrianglesStore::setNbTriangles(int backup) {
mNbTriangles = backup;
}
// Return the last triangle
inline TriangleEPA& TrianglesStore::last() {
assert(mNbTriangles > 0);
return mTriangles[mNbTriangles - 1];
}
// Create a new triangle
inline TriangleEPA* TrianglesStore::newTriangle(const Vector3* vertices,
uint v0,uint v1, uint v2) {
TriangleEPA* newTriangle = nullptr;
// If we have not reached the maximum number of triangles
if (mNbTriangles != MAX_TRIANGLES) {
newTriangle = &mTriangles[mNbTriangles++];
new (newTriangle) TriangleEPA(v0, v1, v2);
if (!newTriangle->computeClosestPoint(vertices)) {
mNbTriangles--;
newTriangle = nullptr;
}
}
// Return the new triangle
return newTriangle;
}
// Access operator
inline TriangleEPA& TrianglesStore::operator[](int i) {
return mTriangles[i];
// Return the pointer to the next manifold info in the linked-list
inline ContactManifoldInfo* ContactManifoldInfo::getNext() {
return mNext;
}
}
#endif

359
src/collision/ContactManifoldSet.cpp
View File

@ -30,10 +30,12 @@ using namespace reactphysics3d;
// Constructor
ContactManifoldSet::ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, int nbMaxManifolds)
: mNbMaxManifolds(nbMaxManifolds), mNbManifolds(0), mShape1(shape1),
mShape2(shape2), mMemoryAllocator(memoryAllocator) {
assert(nbMaxManifolds >= 1);
MemoryAllocator& memoryAllocator)
: mNbManifolds(0), mShape1(shape1),
mShape2(shape2), mMemoryAllocator(memoryAllocator), mManifolds(nullptr) {
// Compute the maximum number of manifolds allowed between the two shapes
mNbMaxManifolds = computeNbMaxContactManifolds(shape1->getCollisionShape(), shape2->getCollisionShape());
}
// Destructor
@ -43,194 +45,239 @@ ContactManifoldSet::~ContactManifoldSet() {
clear();
}
// Add a contact point to the manifold set
void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
void ContactManifoldSet::addContactManifold(const ContactManifoldInfo* contactManifoldInfo) {
// Compute an Id corresponding to the normal direction (using a cubemap)
short int normalDirectionId = computeCubemapNormalId(contact->getNormal());
assert(contactManifoldInfo->getFirstContactPointInfo() != nullptr);
// If there is no contact manifold yet
if (mNbManifolds == 0) {
createManifold(normalDirectionId);
mManifolds[0]->addContactPoint(contact);
assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0);
for (int i=0; i<mNbManifolds; i++) {
assert(mManifolds[i]->getNbContactPoints() > 0);
}
return;
}
// Select the manifold with the most similar normal (if exists)
int similarManifoldIndex = 0;
if (mNbMaxManifolds > 1) {
similarManifoldIndex = selectManifoldWithSimilarNormal(normalDirectionId);
}
// Try to find an existing contact manifold with similar contact normal
ContactManifold* similarManifold = selectManifoldWithSimilarNormal(contactManifoldInfo);
// If a similar manifold has been found
if (similarManifoldIndex != -1) {
if (similarManifold != nullptr) {
// Add the contact point to that similar manifold
mManifolds[similarManifoldIndex]->addContactPoint(contact);
assert(mManifolds[similarManifoldIndex]->getNbContactPoints() > 0);
return;
}
// If the maximum number of manifold has not been reached yet
if (mNbManifolds < mNbMaxManifolds) {
// Create a new manifold for the contact point
createManifold(normalDirectionId);
mManifolds[mNbManifolds-1]->addContactPoint(contact);
for (int i=0; i<mNbManifolds; i++) {
assert(mManifolds[i]->getNbContactPoints() > 0);
}
return;
}
// The contact point will be in a new contact manifold, we now have too much
// manifolds condidates. We need to remove one. We choose to keep the manifolds
// with the largest contact depth among their points
int smallestDepthIndex = -1;
decimal minDepth = contact->getPenetrationDepth();
assert(mNbManifolds == mNbMaxManifolds);
for (int i=0; i<mNbManifolds; i++) {
decimal depth = mManifolds[i]->getLargestContactDepth();
if (depth < minDepth) {
minDepth = depth;
smallestDepthIndex = i;
}
}
// If we do not want to keep to new manifold (not created yet) with the
// new contact point
if (smallestDepthIndex == -1) {
// Delete the new contact
contact->~ContactPoint();
mMemoryAllocator.release(contact, sizeof(ContactPoint));
return;
}
assert(smallestDepthIndex >= 0 && smallestDepthIndex < mNbManifolds);
// Here we need to replace an existing manifold with a new one (that contains
// the new contact point)
removeManifold(smallestDepthIndex);
createManifold(normalDirectionId);
mManifolds[mNbManifolds-1]->addContactPoint(contact);
assert(mManifolds[mNbManifolds-1]->getNbContactPoints() > 0);
for (int i=0; i<mNbManifolds; i++) {
assert(mManifolds[i]->getNbContactPoints() > 0);
}
return;
}
// Return the index of the contact manifold with a similar average normal.
// If no manifold has close enough average normal, it returns -1
int ContactManifoldSet::selectManifoldWithSimilarNormal(short int normalDirectionId) const {
// Return the Id of the manifold with the same normal direction id (if exists)
for (int i=0; i<mNbManifolds; i++) {
if (normalDirectionId == mManifolds[i]->getNormalDirectionId()) {
return i;
}
}
return -1;
}
// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
// Each face of the cube is divided into 4x4 buckets. This method maps the
// normal vector into of the of the bucket and returns a unique Id for the bucket
short int ContactManifoldSet::computeCubemapNormalId(const Vector3& normal) const {
assert(normal.lengthSquare() > MACHINE_EPSILON);
int faceNo;
decimal u, v;
decimal max = max3(fabs(normal.x), fabs(normal.y), fabs(normal.z));
Vector3 normalScaled = normal / max;
if (normalScaled.x >= normalScaled.y && normalScaled.x >= normalScaled.z) {
faceNo = normalScaled.x > 0 ? 0 : 1;
u = normalScaled.y;
v = normalScaled.z;
}
else if (normalScaled.y >= normalScaled.x && normalScaled.y >= normalScaled.z) {
faceNo = normalScaled.y > 0 ? 2 : 3;
u = normalScaled.x;
v = normalScaled.z;
// Update the old manifold with the new one
updateManifoldWithNewOne(similarManifold, contactManifoldInfo);
}
else {
faceNo = normalScaled.z > 0 ? 4 : 5;
u = normalScaled.x;
v = normalScaled.y;
// Create a new contact manifold
createManifold(contactManifoldInfo);
}
int indexU = floor(((u + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
int indexV = floor(((v + 1)/2) * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS);
if (indexU == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexU--;
if (indexV == CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS) indexV--;
const int nbSubDivInFace = CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS * CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS;
return faceNo * 200 + indexU * nbSubDivInFace + indexV;
}
// Update the contact manifolds
void ContactManifoldSet::update() {
// Update a previous similar manifold with a new one
void ContactManifoldSet::updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold) {
for (int i=mNbManifolds-1; i>=0; i--) {
assert(oldManifold != nullptr);
assert(newManifold != nullptr);
// Update the contact manifold
mManifolds[i]->update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(),
mShape2->getBody()->getTransform() * mShape2->getLocalToBodyTransform());
// For each contact point of the new manifold
ContactPointInfo* contactPointInfo = newManifold->getFirstContactPointInfo();
assert(contactPointInfo != nullptr);
while (contactPointInfo != nullptr) {
// Remove the contact manifold if has no contact points anymore
if (mManifolds[i]->getNbContactPoints() == 0) {
removeManifold(i);
// For each contact point in the old manifold
bool isSimilarPointFound = false;
ContactPoint* oldContactPoint = oldManifold->getContactPoints();
while (oldContactPoint != nullptr) {
assert(oldContactPoint != nullptr);
// If the new contact point is similar (very close) to the old contact point
if (oldContactPoint->isSimilarWithContactPoint(contactPointInfo)) {
// Replace (update) the old contact point with the new one
oldContactPoint->update(contactPointInfo);
isSimilarPointFound = true;
break;
}
oldContactPoint = oldContactPoint->getNext();
}
// If we have not found a similar contact point
if (!isSimilarPointFound) {
// Add the contact point to the manifold
oldManifold->addContactPoint(contactPointInfo);
}
contactPointInfo = contactPointInfo->next;
}
// The old manifold is no longer obsolete
oldManifold->setIsObsolete(false, false);
}
// Remove a contact manifold that is the least optimal (smaller penetration depth)
void ContactManifoldSet::removeNonOptimalManifold() {
assert(mNbManifolds > mNbMaxManifolds);
assert(mManifolds != nullptr);
// Look for a manifold that is not new and with the smallest contact penetration depth.
// At the same time, we also look for a new manifold with the smallest contact penetration depth
// in case no old manifold exists.
ContactManifold* minDepthManifold = nullptr;
decimal minDepth = DECIMAL_LARGEST;
ContactManifold* manifold = mManifolds;
while (manifold != nullptr) {
// Get the largest contact point penetration depth of the manifold
const decimal depth = manifold->getLargestContactDepth();
if (depth < minDepth) {
minDepth = depth;
minDepthManifold = manifold;
}
manifold = manifold->getNext();
}
// Remove the non optimal manifold
assert(minDepthManifold != nullptr);
removeManifold(minDepthManifold);
}
// Return the contact manifold with a similar contact normal.
// If no manifold has close enough contact normal, it returns nullptr
ContactManifold* ContactManifoldSet::selectManifoldWithSimilarNormal(const ContactManifoldInfo* contactManifold) const {
// Get the contact normal of the first point of the manifold
const ContactPointInfo* contactPoint = contactManifold->getFirstContactPointInfo();
assert(contactPoint != nullptr);
ContactManifold* manifold = mManifolds;
// Return the Id of the manifold with the same normal direction id (if exists)
while (manifold != nullptr) {
// Get the first contact point of the current manifold
const ContactPoint* point = manifold->getContactPoints();
assert(point != nullptr);
// If the contact normal of the two manifolds are close enough
if (contactPoint->normal.dot(point->getNormal()) >= COS_ANGLE_SIMILAR_CONTACT_MANIFOLD) {
return manifold;
}
manifold = manifold->getNext();
}
return nullptr;
}
// Clear the contact manifold set
void ContactManifoldSet::clear() {
// Destroy all the contact manifolds
for (int i=mNbManifolds-1; i>=0; i--) {
removeManifold(i);
ContactManifold* manifold = mManifolds;
while(manifold != nullptr) {
ContactManifold* nextManifold = manifold->getNext();
// Delete the contact manifold
manifold->~ContactManifold();
mMemoryAllocator.release(manifold, sizeof(ContactManifold));
manifold = nextManifold;
mNbManifolds--;
}
assert(mNbManifolds == 0);
}
// Create a new contact manifold and add it to the set
void ContactManifoldSet::createManifold(short int normalDirectionId) {
assert(mNbManifolds < mNbMaxManifolds);
void ContactManifoldSet::createManifold(const ContactManifoldInfo* manifoldInfo) {
ContactManifold* manifold = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
ContactManifold(manifoldInfo, mShape1, mShape2, mMemoryAllocator);
manifold->setPrevious(nullptr);
manifold->setNext(mManifolds);
if (mManifolds != nullptr) {
mManifolds->setPrevious(manifold);
}
mManifolds = manifold;
mManifolds[mNbManifolds] = new (mMemoryAllocator.allocate(sizeof(ContactManifold)))
ContactManifold(mShape1, mShape2, mMemoryAllocator, normalDirectionId);
mNbManifolds++;
}
// Remove a contact manifold from the set
void ContactManifoldSet::removeManifold(int index) {
void ContactManifoldSet::removeManifold(ContactManifold* manifold) {
assert(mNbManifolds > 0);
assert(index >= 0 && index < mNbManifolds);
assert(manifold != nullptr);
// Delete the new contact
mManifolds[index]->~ContactManifold();
mMemoryAllocator.release(mManifolds[index], sizeof(ContactManifold));
ContactManifold* previous = manifold->getPrevious();
ContactManifold* next = manifold->getNext();
for (int i=index; (i+1) < mNbManifolds; i++) {
mManifolds[i] = mManifolds[i+1];
if (previous != nullptr) {
previous->setNext(next);
}
else {
mManifolds = next;
}
if (next != nullptr) {
next->setPrevious(previous);
}
// Delete the contact manifold
manifold->~ContactManifold();
mMemoryAllocator.release(manifold, sizeof(ContactManifold));
mNbManifolds--;
}
// Make all the contact manifolds and contact points obsolete
void ContactManifoldSet::makeContactsObsolete() {
ContactManifold* manifold = mManifolds;
while (manifold != nullptr) {
manifold->setIsObsolete(true, true);
manifold = manifold->getNext();
}
}
// Clear the obsolete contact manifolds and contact points
void ContactManifoldSet::clearObsoleteManifoldsAndContactPoints() {
ContactManifold* manifold = mManifolds;
while (manifold != nullptr) {
// Get the next manifold in the linked-list
ContactManifold* nextManifold = manifold->getNext();
// If the manifold is obsolete
if (manifold->getIsObsolete()) {
// Delete the contact manifold
removeManifold(manifold);
}
else {
// Clear the obsolete contact points of the manifold
manifold->clearObsoleteContactPoints();
}
manifold = nextManifold;
}
}
// Remove some contact manifolds and contact points if there are too many of them
void ContactManifoldSet::reduce() {
// Remove non optimal contact manifold while there are too many manifolds in the set
while (mNbManifolds > mNbMaxManifolds) {
removeNonOptimalManifold();
}
// Reduce all the contact manifolds in case they have too many contact points
ContactManifold* manifold = mManifolds;
while (manifold != nullptr) {
manifold->reduce();
manifold = manifold->getNext();
}
}

85
src/collision/ContactManifoldSet.h
View File

@ -60,27 +60,34 @@ class ContactManifoldSet {
/// Pointer to the second proxy shape of the contact
ProxyShape* mShape2;
/// Reference to the memory allocator
/// Reference to the memory allocator for the contact manifolds
MemoryAllocator& mMemoryAllocator;
/// Contact manifolds of the set
ContactManifold* mManifolds[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET];
ContactManifold* mManifolds;
// -------------------- Methods -------------------- //
/// Create a new contact manifold and add it to the set
void createManifold(short normalDirectionId);
void createManifold(const ContactManifoldInfo* manifoldInfo);
/// Remove a contact manifold from the set
void removeManifold(int index);
// Return the contact manifold with a similar contact normal.
ContactManifold* selectManifoldWithSimilarNormal(const ContactManifoldInfo* contactManifold) const;
// Return the index of the contact manifold with a similar average normal.
int selectManifoldWithSimilarNormal(short int normalDirectionId) const;
/// Remove a contact manifold that is the least optimal (smaller penetration depth)
void removeNonOptimalManifold();
// Map the normal vector into a cubemap face bucket (a face contains 4x4 buckets)
// Each face of the cube is divided into 4x4 buckets. This method maps the
// normal vector into of the of the bucket and returns a unique Id for the bucket
short int computeCubemapNormalId(const Vector3& normal) const;
/// Update a previous similar manifold with a new one
void updateManifoldWithNewOne(ContactManifold* oldManifold, const ContactManifoldInfo* newManifold);
/// Return the maximum number of contact manifolds allowed between to collision shapes
int computeNbMaxContactManifolds(const CollisionShape* shape1, const CollisionShape* shape2);
/// Clear the contact manifold set
void clear();
/// Delete a contact manifold
void removeManifold(ContactManifold* manifold);
public:
@ -88,34 +95,37 @@ class ContactManifoldSet {
/// Constructor
ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, int nbMaxManifolds);
MemoryAllocator& memoryAllocator);
/// Destructor
~ContactManifoldSet();
/// Add a contact manifold in the set
void addContactManifold(const ContactManifoldInfo* contactManifoldInfo);
/// Return the first proxy shape
ProxyShape* getShape1() const;
/// Return the second proxy shape
ProxyShape* getShape2() const;
/// Add a contact point to the manifold set
void addContactPoint(ContactPoint* contact);
/// Update the contact manifolds
void update();
/// Clear the contact manifold set
void clear();
/// Return the number of manifolds in the set
int getNbContactManifolds() const;
/// Return a given contact manifold
ContactManifold* getContactManifold(int index) const;
/// Return a pointer to the first element of the linked-list of contact manifolds
ContactManifold* getContactManifolds() const;
/// Make all the contact manifolds and contact points obsolete
void makeContactsObsolete();
/// Return the total number of contact points in the set of manifolds
int getTotalNbContactPoints() const;
/// Clear the obsolete contact manifolds and contact points
void clearObsoleteManifoldsAndContactPoints();
// Remove some contact manifolds and contact points if there are too many of them
void reduce();
};
// Return the first proxy shape
@ -133,21 +143,38 @@ inline int ContactManifoldSet::getNbContactManifolds() const {
return mNbManifolds;
}
// Return a given contact manifold
inline ContactManifold* ContactManifoldSet::getContactManifold(int index) const {
assert(index >= 0 && index < mNbManifolds);
return mManifolds[index];
// Return a pointer to the first element of the linked-list of contact manifolds
inline ContactManifold* ContactManifoldSet::getContactManifolds() const {
return mManifolds;
}
// Return the total number of contact points in the set of manifolds
inline int ContactManifoldSet::getTotalNbContactPoints() const {
int nbPoints = 0;
for (int i=0; i<mNbManifolds; i++) {
nbPoints += mManifolds[i]->getNbContactPoints();
ContactManifold* manifold = mManifolds;
while (manifold != nullptr) {
nbPoints += manifold->getNbContactPoints();
manifold = manifold->getNext();
}
return nbPoints;
}
// Return the maximum number of contact manifolds allowed between to collision shapes
inline int ContactManifoldSet::computeNbMaxContactManifolds(const CollisionShape* shape1, const CollisionShape* shape2) {
// If both shapes are convex
if (shape1->isConvex() && shape2->isConvex()) {
return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE;
} // If there is at least one concave shape
else {
return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE;
}
}
}
#endif

63
src/engine/Impulse.h → src/collision/ContactPointInfo.h
View File

@ -23,19 +23,25 @@
* *
********************************************************************************/
#ifndef REACTPHYSICS3D_IMPULSE_H
#define REACTPHYSICS3D_IMPULSE_H
#ifndef REACTPHYSICS3D_CONTACT_POINT_INFO_H
#define REACTPHYSICS3D_CONTACT_POINT_INFO_H
// Libraries
#include "body/CollisionBody.h"
#include "mathematics/mathematics.h"
#include "configuration.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Structure Impulse
// Structure ContactPointInfo
/**
* Represents an impulse that we can apply to bodies in the contact or constraint solver.
* This structure contains informations about a collision contact
* computed during the narrow-phase collision detection. Those
* informations are used to compute the contact set for a contact
* between two bodies.
*/
struct Impulse {
struct ContactPointInfo {
private:
@ -45,41 +51,38 @@ struct Impulse {
// -------------------- Attributes -------------------- //
/// Linear impulse applied to the first body
const Vector3 linearImpulseBody1;
/// Normalized normal vector of the collision contact in world space
Vector3 normal;
/// Angular impulse applied to the first body
const Vector3 angularImpulseBody1;
/// Penetration depth of the contact
decimal penetrationDepth;
/// Linear impulse applied to the second body
const Vector3 linearImpulseBody2;
/// Contact point of body 1 in local space of body 1
Vector3 localPoint1;
/// Angular impulse applied to the second body
const Vector3 angularImpulseBody2;
/// Contact point of body 2 in local space of body 2
Vector3 localPoint2;
/// Pointer to the next contact point info
ContactPointInfo* next;
/// True if the contact point has already been inserted into a manifold
bool isUsed;
// -------------------- Methods -------------------- //
/// Constructor
Impulse(const Vector3& initLinearImpulseBody1, const Vector3& initAngularImpulseBody1,
const Vector3& initLinearImpulseBody2, const Vector3& initAngularImpulseBody2)
: linearImpulseBody1(initLinearImpulseBody1),
angularImpulseBody1(initAngularImpulseBody1),
linearImpulseBody2(initLinearImpulseBody2),
angularImpulseBody2(initAngularImpulseBody2) {
ContactPointInfo(const Vector3& contactNormal, decimal penDepth,
const Vector3& localPt1, const Vector3& localPt2)
: normal(contactNormal), penetrationDepth(penDepth),
localPoint1(localPt1), localPoint2(localPt2), next(nullptr), isUsed(false) {
assert(contactNormal.lengthSquare() > decimal(0.8));
assert(penDepth > decimal(0.0));
}
/// Copy-constructor
Impulse(const Impulse& impulse)
: linearImpulseBody1(impulse.linearImpulseBody1),
angularImpulseBody1(impulse.angularImpulseBody1),
linearImpulseBody2(impulse.linearImpulseBody2),
angularImpulseBody2(impulse.angularImpulseBody2) {
}
/// Deleted assignment operator
Impulse& operator=(const Impulse& impulse) = delete;
/// Destructor
~ContactPointInfo() = default;
};
}

116
src/collision/HalfEdgeStructure.cpp Normal file
View File

@ -0,0 +1,116 @@
/********************************************************************************
* 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 "HalfEdgeStructure.h"
#include <map>
using namespace reactphysics3d;
// Initialize the structure (when all vertices and faces have been added)
void HalfEdgeStructure::init() {
using edgeKey = std::pair<uint, uint>;
std::map<edgeKey, Edge> edges;
std::map<edgeKey, edgeKey> nextEdges;
std::map<edgeKey, uint> mapEdgeToStartVertex;
std::map<edgeKey, uint> mapEdgeToIndex;
std::map<uint, edgeKey> mapEdgeIndexToKey;
std::map<uint, edgeKey> mapFaceIndexToEdgeKey;
// For each face
for (uint f=0; f<mFaces.size(); f++) {
Face face = mFaces[f];
std::vector<edgeKey> currentFaceEdges;
edgeKey firstEdgeKey;
// For each vertex of the face
for (uint v=0; v < face.faceVertices.size(); v++) {
uint v1Index = face.faceVertices[v];
uint v2Index = face.faceVertices[v == (face.faceVertices.size() - 1) ? 0 : v + 1];
const edgeKey pairV1V2 = std::make_pair(v1Index, v2Index);
// Create a new half-edge
Edge edge;
edge.faceIndex = f;
edge.vertexIndex = v1Index;
if (v == 0) {
firstEdgeKey = pairV1V2;
}
else if (v >= 1) {
nextEdges.insert(std::make_pair(currentFaceEdges[currentFaceEdges.size() - 1], pairV1V2));
}
if (v == (face.faceVertices.size() - 1)) {
nextEdges.insert(std::make_pair(pairV1V2, firstEdgeKey));
}
edges.insert(std::make_pair(pairV1V2, edge));
const edgeKey pairV2V1 = std::make_pair(v2Index, v1Index);
mapEdgeToStartVertex.insert(std::make_pair(pairV1V2, v1Index));
mapEdgeToStartVertex.insert(std::make_pair(pairV2V1, v2Index));
mapFaceIndexToEdgeKey.insert(std::make_pair(f, pairV1V2));
auto itEdge = edges.find(pairV2V1);
if (itEdge != edges.end()) {
const uint edgeIndex = mEdges.size();
itEdge->second.twinEdgeIndex = edgeIndex + 1;
edge.twinEdgeIndex = edgeIndex;
mapEdgeIndexToKey[edgeIndex] = pairV2V1;
mapEdgeIndexToKey[edgeIndex + 1] = pairV1V2;
mVertices[v1Index].edgeIndex = edgeIndex + 1;
mVertices[v2Index].edgeIndex = edgeIndex;
mapEdgeToIndex.insert(std::make_pair(pairV1V2, edgeIndex + 1));
mapEdgeToIndex.insert(std::make_pair(pairV2V1, edgeIndex));
mEdges.add(itEdge->second);
mEdges.add(edge);
}
currentFaceEdges.push_back(pairV1V2);
}
}
// Set next edges
for (uint i=0; i < mEdges.size(); i++) {
mEdges[i].nextEdgeIndex = mapEdgeToIndex[nextEdges[mapEdgeIndexToKey[i]]];
}
// Set face edge
for (uint f=0; f < mFaces.size(); f++) {
mFaces[f].edgeIndex = mapEdgeToIndex[mapFaceIndexToEdgeKey[f]];
}
}

175
src/collision/HalfEdgeStructure.h Normal file
View File

@ -0,0 +1,175 @@
/********************************************************************************
* 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_HALF_EDGE_STRUCTURE_MESH_H
#define REACTPHYSICS3D_HALF_EDGE_STRUCTURE_MESH_H
// Libraries
#include "mathematics/mathematics.h"
#include <vector>
namespace reactphysics3d {
// Class HalfEdgeStructure
/**
* This class describes a polyhedron mesh made of faces and vertices.
* The faces do not have to be triangle. Note that the half-edge structure
* is only valid if the mesh is closed (each edge has two adjacent faces).
*/
class HalfEdgeStructure {
public:
struct Edge {
uint vertexIndex; // Index of the vertex at the beginning of the edge
uint twinEdgeIndex; // Index of the twin edge
uint faceIndex; // Adjacent face index of the edge
uint nextEdgeIndex; // Index of the next edge
};
struct Face {
uint edgeIndex; // Index of an half-edge of the face
List<uint> faceVertices; // Index of the vertices of the face
/// Constructor
Face(MemoryAllocator& allocator) : faceVertices(allocator) {}
/// Constructor
Face(List<uint> vertices) : faceVertices(vertices) {}
};
struct Vertex {
uint vertexPointIndex; // Index of the vertex point in the origin vertex array
uint edgeIndex; // Index of one edge emanting from this vertex
/// Constructor
Vertex(uint vertexCoordsIndex) : vertexPointIndex(vertexCoordsIndex) { }
};
private:
/// Reference to a memory allocator
MemoryAllocator& mAllocator;
/// All the faces
List<Face> mFaces;
/// All the vertices
List<Vertex> mVertices;
/// All the half-edges
List<Edge> mEdges;
public:
/// Constructor
HalfEdgeStructure(MemoryAllocator& allocator, uint facesCapacity, uint verticesCapacity,
uint edgesCapacity) :mAllocator(allocator), mFaces(allocator, facesCapacity),
mVertices(allocator, verticesCapacity), mEdges(allocator, edgesCapacity) {}
/// Destructor
~HalfEdgeStructure() = default;
/// Initialize the structure (when all vertices and faces have been added)
void init();
/// Add a vertex
uint addVertex(uint vertexPointIndex);
/// Add a face
void addFace(List<uint> faceVertices);
/// Return the number of faces
uint getNbFaces() const;
/// Return the number of half-edges
uint getNbHalfEdges() const;
/// Return the number of vertices
uint getNbVertices() const;
/// Return a given face
const Face& getFace(uint index) const;
/// Return a given edge
const Edge& getHalfEdge(uint index) const;
/// Return a given vertex
const Vertex& getVertex(uint index) const;
};
// Add a vertex
inline uint HalfEdgeStructure::addVertex(uint vertexPointIndex) {
Vertex vertex(vertexPointIndex);
mVertices.add(vertex);
return mVertices.size() - 1;
}
// Add a face
inline void HalfEdgeStructure::addFace(List<uint> faceVertices) {
// Create a new face
Face face(faceVertices);
mFaces.add(face);
}
// Return the number of faces
inline uint HalfEdgeStructure::getNbFaces() const {
return static_cast<uint>(mFaces.size());
}
// Return the number of edges
inline uint HalfEdgeStructure::getNbHalfEdges() const {
return static_cast<uint>(mEdges.size());
}
// Return the number of vertices
inline uint HalfEdgeStructure::getNbVertices() const {
return static_cast<uint>(mVertices.size());
}
// Return a given face
inline const HalfEdgeStructure::Face& HalfEdgeStructure::getFace(uint index) const {
assert(index < mFaces.size());
return mFaces[index];
}
// Return a given edge
inline const HalfEdgeStructure::Edge& HalfEdgeStructure::getHalfEdge(uint index) const {
assert(index < mEdges.size());
return mEdges[index];
}
// Return a given vertex
inline const HalfEdgeStructure::Vertex& HalfEdgeStructure::getVertex(uint index) const {
assert(index < mVertices.size());
return mVertices[index];
}
}
#endif

60
src/collision/MiddlePhaseTriangleCallback.cpp Normal file
View File

@ -0,0 +1,60 @@
/********************************************************************************
* 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 "collision/MiddlePhaseTriangleCallback.h"
using namespace reactphysics3d;
// Report collision between a triangle of a concave shape and the convex mesh shape (for middle-phase)
void MiddlePhaseTriangleCallback::testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) {
// Create a triangle collision shape (the allocated memory for the TriangleShape will be released in the
// destructor of the corresponding NarrowPhaseInfo.
TriangleShape* triangleShape = new (mAllocator.allocate(sizeof(TriangleShape)))
TriangleShape(trianglePoints, verticesNormals, shapeId, mAllocator);
#ifdef IS_PROFILING_ACTIVE
// Set the profiler to the triangle shape
triangleShape->setProfiler(mProfiler);
#endif
bool isShape1Convex = mOverlappingPair->getShape1()->getCollisionShape()->isConvex();
ProxyShape* shape1 = isShape1Convex ? mConvexProxyShape : mConcaveProxyShape;
ProxyShape* shape2 = isShape1Convex ? mConcaveProxyShape : mConvexProxyShape;
// Create a narrow phase info for the narrow-phase collision detection
NarrowPhaseInfo* firstNarrowPhaseInfo = narrowPhaseInfoList;
narrowPhaseInfoList = new (mAllocator.allocate(sizeof(NarrowPhaseInfo)))
NarrowPhaseInfo(mOverlappingPair,
isShape1Convex ? mConvexProxyShape->getCollisionShape() : triangleShape,
isShape1Convex ? triangleShape : mConvexProxyShape->getCollisionShape(),
shape1->getLocalToWorldTransform(),
shape2->getLocalToWorldTransform(),
mAllocator);
narrowPhaseInfoList->next = firstNarrowPhaseInfo;
}

100
src/collision/MiddlePhaseTriangleCallback.h Normal file
View File

@ -0,0 +1,100 @@
/********************************************************************************
* 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_MIDDLE_PHASE_TRIANGLE_CALLBACK_H
#define REACTPHYSICS3D_MIDDLE_PHASE_TRIANGLE_CALLBACK_H
// Libraries
#include "collision/shapes/ConcaveShape.h"
#include "collision/narrowphase/NarrowPhaseAlgorithm.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Class ConvexVsTriangleCallback
/**
* This class is used to report a collision between the triangle
* of a concave mesh shape and a convex shape during the
* middle-phase algorithm.
*/
class MiddlePhaseTriangleCallback : public TriangleCallback {
protected:
/// Broadphase overlapping pair
OverlappingPair* mOverlappingPair;
/// Pointer to the concave proxy shape
ProxyShape* mConcaveProxyShape;
/// Pointer to the convex proxy shape
ProxyShape* mConvexProxyShape;
/// Pointer to the concave collision shape
const ConcaveShape* mConcaveShape;
/// Reference to the single-frame memory allocator
MemoryAllocator& mAllocator;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
public:
/// Pointer to the first element of the linked-list of narrow-phase info
NarrowPhaseInfo* narrowPhaseInfoList;
/// Constructor
MiddlePhaseTriangleCallback(OverlappingPair* overlappingPair,
ProxyShape* concaveProxyShape,
ProxyShape* convexProxyShape, const ConcaveShape* concaveShape,
MemoryAllocator& allocator)
:mOverlappingPair(overlappingPair), mConcaveProxyShape(concaveProxyShape),
mConvexProxyShape(convexProxyShape), mConcaveShape(concaveShape),
mAllocator(allocator), narrowPhaseInfoList(nullptr) {
}
/// Test collision between a triangle and the convex mesh shape
virtual void testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) override;
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
void setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
#endif
};
}
#endif

110
src/collision/NarrowPhaseInfo.cpp Normal file
View File

@ -0,0 +1,110 @@
/********************************************************************************
* 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 <iostream>
#include "NarrowPhaseInfo.h"
#include "ContactPointInfo.h"
#include "collision/shapes/TriangleShape.h"
#include "engine/OverlappingPair.h"
using namespace reactphysics3d;
// Constructor
NarrowPhaseInfo::NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1,
CollisionShape* shape2, const Transform& shape1Transform,
const Transform& shape2Transform, MemoryAllocator& shapeAllocator)
: overlappingPair(pair), collisionShape1(shape1), collisionShape2(shape2),
shape1ToWorldTransform(shape1Transform), shape2ToWorldTransform(shape2Transform),
contactPoints(nullptr), next(nullptr), collisionShapeAllocator(shapeAllocator) {
// Add a collision info for the two collision shapes into the overlapping pair (if not present yet)
overlappingPair->addLastFrameInfoIfNecessary(shape1->getId(), shape2->getId());
}
// Destructor
NarrowPhaseInfo::~NarrowPhaseInfo() {
assert(contactPoints == nullptr);
// Release the memory of the TriangleShape (this memory was allocated in the
// MiddlePhaseTriangleCallback::testTriangle() method)
if (collisionShape1->getName() == CollisionShapeName::TRIANGLE) {
collisionShape1->~CollisionShape();
collisionShapeAllocator.release(collisionShape1, sizeof(TriangleShape));
}
if (collisionShape2->getName() == CollisionShapeName::TRIANGLE) {
collisionShape2->~CollisionShape();
collisionShapeAllocator.release(collisionShape2, sizeof(TriangleShape));
}
}
// Add a new contact point
void NarrowPhaseInfo::addContactPoint(const Vector3& contactNormal, decimal penDepth,
const Vector3& localPt1, const Vector3& localPt2) {
assert(penDepth > decimal(0.0));
// Get the memory allocator
MemoryAllocator& allocator = overlappingPair->getTemporaryAllocator();
// Create the contact point info
ContactPointInfo* contactPointInfo = new (allocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(contactNormal, penDepth, localPt1, localPt2);
// Add it into the linked list of contact points
contactPointInfo->next = contactPoints;
contactPoints = contactPointInfo;
}
/// Take all the generated contact points and create a new potential
/// contact manifold into the overlapping pair
void NarrowPhaseInfo::addContactPointsAsPotentialContactManifold() {
overlappingPair->addPotentialContactPoints(this);
}
// Reset the remaining contact points
void NarrowPhaseInfo::resetContactPoints() {
// Get the memory allocator
MemoryAllocator& allocator = overlappingPair->getTemporaryAllocator();
// For each remaining contact point info
ContactPointInfo* element = contactPoints;
while(element != nullptr) {
ContactPointInfo* elementToDelete = element;
element = element->next;
// Call the destructor
elementToDelete->~ContactPointInfo();
// Delete the current element
allocator.release(elementToDelete, sizeof(ContactPointInfo));
}
contactPoints = nullptr;
}

102
src/collision/NarrowPhaseInfo.h Normal file
View File

@ -0,0 +1,102 @@
/********************************************************************************
* 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_NARROW_PHASE_INFO_H
#define REACTPHYSICS3D_NARROW_PHASE_INFO_H
// Libraries
#include "shapes/CollisionShape.h"
#include "collision/ContactManifoldInfo.h"
#include "engine/OverlappingPair.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
struct LastFrameCollisionInfo;
// Class NarrowPhaseInfo
/**
* This structure regroups different things about a collision shape. This is
* used to pass information about a collision shape to a collision algorithm.
*/
struct NarrowPhaseInfo {
public:
/// Broadphase overlapping pair
OverlappingPair* overlappingPair;
/// Pointer to the first collision shape to test collision with
CollisionShape* collisionShape1;
/// Pointer to the second collision shape to test collision with
CollisionShape* collisionShape2;
/// Transform that maps from collision shape 1 local-space to world-space
Transform shape1ToWorldTransform;
/// Transform that maps from collision shape 2 local-space to world-space
Transform shape2ToWorldTransform;
/// Linked-list of contact points created during the narrow-phase
ContactPointInfo* contactPoints;
/// Pointer to the next element in the linked list
NarrowPhaseInfo* next;
/// Memory allocator for the collision shape (Used to release TriangleShape memory in destructor)
MemoryAllocator& collisionShapeAllocator;
/// Constructor
NarrowPhaseInfo(OverlappingPair* pair, CollisionShape* shape1,
CollisionShape* shape2, const Transform& shape1Transform,
const Transform& shape2Transform, MemoryAllocator& shapeAllocator);
/// Destructor
~NarrowPhaseInfo();
/// Add a new contact point
void addContactPoint(const Vector3& contactNormal, decimal penDepth,
const Vector3& localPt1, const Vector3& localPt2);
/// Create a new potential contact manifold into the overlapping pair using current contact points
void addContactPointsAsPotentialContactManifold();
/// Reset the remaining contact points
void resetContactPoints();
/// Get the last collision frame info for temporal coherence
LastFrameCollisionInfo* getLastFrameCollisionInfo() const;
};
// Get the last collision frame info for temporal coherence
inline LastFrameCollisionInfo* NarrowPhaseInfo::getLastFrameCollisionInfo() const {
return overlappingPair->getLastFrameCollisionInfo(collisionShape1->getId(), collisionShape2->getId());
}
}
#endif

57
src/collision/OverlapCallback.h Normal file
View File

@ -0,0 +1,57 @@
/********************************************************************************
* 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_OVERLAP_CALLBACK_H
#define REACTPHYSICS3D_OVERLAP_CALLBACK_H
// Libraries
#include "body/CollisionBody.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Class OverlapCallback
/**
* This class can be used to register a callback for collision overlap queries.
* You should implement your own class inherited from this one and implement
* the notifyOverlap() method. This method will called each time a contact
* point is reported.
*/
class OverlapCallback {
public:
/// Destructor
virtual ~OverlapCallback() {
}
/// This method will be called for each reported overlapping bodies
virtual void notifyOverlap(CollisionBody* collisionBody)=0;
};
}
#endif

75
src/collision/PolygonVertexArray.cpp Normal file
View File

@ -0,0 +1,75 @@
/********************************************************************************
* 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 "PolygonVertexArray.h"
using namespace reactphysics3d;
// Constructor
/// Note that your data will not be copied into the PolygonVertexArray and
/// therefore, you need to make sure that those data are always valid during
/// the lifetime of the PolygonVertexArray.
/**
*/
PolygonVertexArray::PolygonVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
void* indexesStart, int indexesStride,
uint nbFaces, PolygonFace* facesStart,
VertexDataType vertexDataType, IndexDataType indexDataType) {
mNbVertices = nbVertices;
mVerticesStart = reinterpret_cast<unsigned char*>(verticesStart);
mVerticesStride = verticesStride;
mIndicesStart = reinterpret_cast<unsigned char*>(indexesStart);
mIndicesStride = indexesStride;
mNbFaces = nbFaces;
mPolygonFacesStart = facesStart;
mVertexDataType = vertexDataType;
mIndexDataType = indexDataType;
}
// Return the vertex index of a given vertex in a face
uint PolygonVertexArray::getVertexIndexInFace(uint faceIndex, uint noVertexInFace) const {
assert(faceIndex < mNbFaces);
// Get the face
PolygonFace* face = getPolygonFace(faceIndex);
assert(noVertexInFace < face->nbVertices);
void* vertexIndexPointer = mIndicesStart + (face->indexBase + noVertexInFace) * mIndicesStride;
if (mIndexDataType == PolygonVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
return *((uint*)vertexIndexPointer);
}
else if (mIndexDataType == PolygonVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
return *((unsigned short*)vertexIndexPointer);
}
else {
assert(false);
}
return 0;
}

188
src/collision/PolygonVertexArray.h Normal file
View File

@ -0,0 +1,188 @@
/********************************************************************************
* 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_POLYGON_VERTEX_ARRAY_H
#define REACTPHYSICS3D_POLYGON_VERTEX_ARRAY_H
// Libraries
#include "configuration.h"
#include <cassert>
namespace reactphysics3d {
// Class PolygonVertexArray
/**
* This class is used to describe the vertices and faces of a polyhedron mesh.
* A PolygonVertexArray represents an array of vertices and polygon faces
* of a polyhedron mesh. When you create a PolygonVertexArray, no data is copied
* into the array. It only stores pointer to the data. The purpose is to allow
* the user to share vertices data between the physics engine and the rendering
* part. Therefore, make sure that the data pointed by a PolygonVertexArray
* remains valid during the PolygonVertexArray life.
*/
class PolygonVertexArray {
public:
/// Data type for the vertices in the array
enum class VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE};
/// Data type for the indices in the array
enum class IndexDataType {INDEX_INTEGER_TYPE, INDEX_SHORT_TYPE};
/// Represent a polygon face of the polyhedron
struct PolygonFace {
/// Number of vertices in the polygon face
uint nbVertices;
/// Index of the first vertex of the polygon face
/// inside the array with all vertex indices
uint indexBase;
};
protected:
/// Number of vertices in the array
uint mNbVertices;
/// Pointer to the first vertex value in the array
unsigned char* mVerticesStart;
/// Stride (number of bytes) between the beginning of two vertices
/// values in the array
int mVerticesStride;
/// Pointer to the first vertex index of the array
unsigned char* mIndicesStart;
/// Stride (number of bytes) between the beginning of two indices in
/// the array
int mIndicesStride;
/// Number of polygon faces in the array
uint mNbFaces;
/// Pointer to the first polygon face in the polyhedron
PolygonFace* mPolygonFacesStart;
/// Data type of the vertices in the array
VertexDataType mVertexDataType;
/// Data type of the indices in the array
IndexDataType mIndexDataType;
public:
/// Constructor
PolygonVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
void* indexesStart, int indexesStride,
uint nbFaces, PolygonFace* facesStart,
VertexDataType vertexDataType, IndexDataType indexDataType);
/// Destructor
~PolygonVertexArray() = default;
/// Return the vertex data type
VertexDataType getVertexDataType() const;
/// Return the index data type
IndexDataType getIndexDataType() const;
/// Return the number of vertices
uint getNbVertices() const;
/// Return the number of faces
uint getNbFaces() const;
/// Return the vertices stride (number of bytes)
int getVerticesStride() const;
/// Return the indices stride (number of bytes)
int getIndicesStride() const;
/// Return the vertex index of a given vertex in a face
uint getVertexIndexInFace(uint faceIndex, uint noVertexInFace) const;
/// Return a polygon face of the polyhedron
PolygonFace* getPolygonFace(uint faceIndex) const;
/// Return the pointer to the start of the vertices array
unsigned char* getVerticesStart() const;
/// Return the pointer to the start of the indices array
unsigned char* getIndicesStart() const;
};
// Return the vertex data type
inline PolygonVertexArray::VertexDataType PolygonVertexArray::getVertexDataType() const {
return mVertexDataType;
}
// Return the index data type
inline PolygonVertexArray::IndexDataType PolygonVertexArray::getIndexDataType() const {
return mIndexDataType;
}
// Return the number of vertices
inline uint PolygonVertexArray::getNbVertices() const {
return mNbVertices;
}
// Return the number of faces
inline uint PolygonVertexArray::getNbFaces() const {
return mNbFaces;
}
// Return the vertices stride (number of bytes)
inline int PolygonVertexArray::getVerticesStride() const {
return mVerticesStride;
}
// Return the indices stride (number of bytes)
inline int PolygonVertexArray::getIndicesStride() const {
return mIndicesStride;
}
// Return a polygon face of the polyhedron
inline PolygonVertexArray::PolygonFace* PolygonVertexArray::getPolygonFace(uint faceIndex) const {
assert(faceIndex < mNbFaces);
return &mPolygonFacesStart[faceIndex];
}
// Return the pointer to the start of the vertices array
inline unsigned char* PolygonVertexArray::getVerticesStart() const {
return mVerticesStart;
}
// Return the pointer to the start of the indices array
inline unsigned char* PolygonVertexArray::getIndicesStart() const {
return mIndicesStart;
}
}
#endif

152
src/collision/PolyhedronMesh.cpp Normal file
View File

@ -0,0 +1,152 @@
/********************************************************************************
* 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 "PolyhedronMesh.h"
#include "memory/MemoryManager.h"
using namespace reactphysics3d;
// Constructor
/*
* Create a polyhedron mesh given an array of polygons.
* @param polygonVertexArray Pointer to the array of polygons and their vertices
*/
PolyhedronMesh::PolyhedronMesh(PolygonVertexArray* polygonVertexArray)
: mHalfEdgeStructure(MemoryManager::getBaseAllocator(),
polygonVertexArray->getNbFaces(),
polygonVertexArray->getNbVertices(),
(polygonVertexArray->getNbFaces() + polygonVertexArray->getNbVertices() - 2) * 2) {
mPolygonVertexArray = polygonVertexArray;
// Create the half-edge structure of the mesh
createHalfEdgeStructure();
// Create the face normals array
mFacesNormals = new Vector3[mHalfEdgeStructure.getNbFaces()];
// Compute the faces normals
computeFacesNormals();
// Compute the centroid
computeCentroid();
}
// Destructor
PolyhedronMesh::~PolyhedronMesh() {
delete[] mFacesNormals;
}
// Create the half-edge structure of the mesh
void PolyhedronMesh::createHalfEdgeStructure() {
// For each vertex of the mesh
for (uint v=0; v < mPolygonVertexArray->getNbVertices(); v++) {
mHalfEdgeStructure.addVertex(v);
}
// For each polygon face of the mesh
for (uint f=0; f < mPolygonVertexArray->getNbFaces(); f++) {
// Get the polygon face
PolygonVertexArray::PolygonFace* face = mPolygonVertexArray->getPolygonFace(f);
List<uint> faceVertices(MemoryManager::getBaseAllocator(), face->nbVertices);
// For each vertex of the face
for (uint v=0; v < face->nbVertices; v++) {
faceVertices.add(mPolygonVertexArray->getVertexIndexInFace(f, v));
}
assert(faceVertices.size() >= 3);
// Addd the face into the half-edge structure
mHalfEdgeStructure.addFace(faceVertices);
}
// Initialize the half-edge structure
mHalfEdgeStructure.init();
}
/// Return a vertex
Vector3 PolyhedronMesh::getVertex(uint index) const {
assert(index < getNbVertices());
// Get the vertex index in the array with all vertices
uint vertexIndex = mHalfEdgeStructure.getVertex(index).vertexPointIndex;
PolygonVertexArray::VertexDataType vertexType = mPolygonVertexArray->getVertexDataType();
unsigned char* verticesStart = mPolygonVertexArray->getVerticesStart();
int vertexStride = mPolygonVertexArray->getVerticesStride();
Vector3 vertex;
if (vertexType == PolygonVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) {
const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
vertex.x = decimal(vertices[0]);
vertex.y = decimal(vertices[1]);
vertex.z = decimal(vertices[2]);
}
else if (vertexType == PolygonVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) {
const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
vertex.x = decimal(vertices[0]);
vertex.y = decimal(vertices[1]);
vertex.z = decimal(vertices[2]);
}
else {
assert(false);
}
return vertex;
}
// Compute the faces normals
void PolyhedronMesh::computeFacesNormals() {
// For each face
for (uint f=0; f < mHalfEdgeStructure.getNbFaces(); f++) {
const HalfEdgeStructure::Face& face = mHalfEdgeStructure.getFace(f);
assert(face.faceVertices.size() >= 3);
const Vector3 vec1 = getVertex(face.faceVertices[1]) - getVertex(face.faceVertices[0]);
const Vector3 vec2 = getVertex(face.faceVertices[2]) - getVertex(face.faceVertices[0]);
mFacesNormals[f] = vec1.cross(vec2);
mFacesNormals[f].normalize();
}
}
// Compute the centroid of the polyhedron
void PolyhedronMesh::computeCentroid() {
mCentroid.setToZero();
for (uint v=0; v < getNbVertices(); v++) {
mCentroid += getVertex(v);
}
mCentroid /= getNbVertices();
}

123
src/collision/PolyhedronMesh.h Normal file
View File

@ -0,0 +1,123 @@
/********************************************************************************
* 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_POLYHEDRON_MESH_H
#define REACTPHYSICS3D_POLYHEDRON_MESH_H
// Libraries
#include "mathematics/mathematics.h"
#include "HalfEdgeStructure.h"
#include "collision/PolygonVertexArray.h"
#include "memory/DefaultAllocator.h"
#include <vector>
namespace reactphysics3d {
// Class PolyhedronMesh
/**
* This class describes a polyhedron mesh made of faces and vertices.
* The faces do not have to be triangles.
*/
class PolyhedronMesh {
private:
// -------------------- Attributes -------------------- //
/// Pointer the the polygon vertex array with vertices and faces
/// of the mesh
PolygonVertexArray* mPolygonVertexArray;
/// Half-edge structure of the mesh
HalfEdgeStructure mHalfEdgeStructure;
/// Array with the face normals
Vector3* mFacesNormals;
/// Centroid of the polyhedron
Vector3 mCentroid;
// -------------------- Methods -------------------- //
/// Create the half-edge structure of the mesh
void createHalfEdgeStructure();
/// Compute the faces normals
void computeFacesNormals();
/// Compute the centroid of the polyhedron
void computeCentroid() ;
public:
// -------------------- Methods -------------------- //
/// Constructor
PolyhedronMesh(PolygonVertexArray* polygonVertexArray);
/// Destructor
~PolyhedronMesh();
/// Return the number of vertices
uint getNbVertices() const;
/// Return a vertex
Vector3 getVertex(uint index) const;
/// Return a face normal
Vector3 getFaceNormal(uint faceIndex) const;
/// Return the half-edge structure of the mesh
const HalfEdgeStructure& getHalfEdgeStructure() const;
/// Return the centroid of the polyhedron
Vector3 getCentroid() const;
};
// Return the number of vertices
inline uint PolyhedronMesh::getNbVertices() const {
return mHalfEdgeStructure.getNbVertices();
}
// Return a face normal
inline Vector3 PolyhedronMesh::getFaceNormal(uint faceIndex) const {
assert(faceIndex < mHalfEdgeStructure.getNbFaces());
return mFacesNormals[faceIndex];
}
// Return the half-edge structure of the mesh
inline const HalfEdgeStructure& PolyhedronMesh::getHalfEdgeStructure() const {
return mHalfEdgeStructure;
}
// Return the centroid of the polyhedron
inline Vector3 PolyhedronMesh::getCentroid() const {
return mCentroid;
}
}
#endif

13
src/collision/ProxyShape.cpp
View File

@ -35,20 +35,15 @@ using namespace reactphysics3d;
* @param transform Transformation from collision shape local-space to body local-space
* @param mass Mass of the collision shape (in kilograms)
*/
ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass)
:mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass),
mNext(nullptr), mBroadPhaseID(-1), mCachedCollisionData(nullptr), mUserData(nullptr),
mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) {
ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass, MemoryManager& memoryManager)
:mMemoryManager(memoryManager), mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass),
mNext(nullptr), mBroadPhaseID(-1), mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) {
}
// Destructor
ProxyShape::~ProxyShape() {
// Release the cached collision data memory
if (mCachedCollisionData != nullptr) {
free(mCachedCollisionData);
}
}
// Return true if a point is inside the collision shape
@ -81,7 +76,7 @@ bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
worldToLocalTransform * ray.point2,
ray.maxFraction);
bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this);
bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this, mMemoryManager.getPoolAllocator());
// Convert the raycast info into world-space
raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint;

86
src/collision/ProxyShape.h
View File

@ -29,6 +29,7 @@
// Libraries
#include "body/CollisionBody.h"
#include "shapes/CollisionShape.h"
#include "memory/MemoryManager.h"
namespace reactphysics3d {
@ -48,6 +49,9 @@ class ProxyShape {
// -------------------- Attributes -------------------- //
/// Reference to the memory manager
MemoryManager& mMemoryManager;
/// Pointer to the parent body
CollisionBody* mBody;
@ -66,9 +70,6 @@ class ProxyShape {
/// Broad-phase ID (node ID in the dynamic AABB tree)
int mBroadPhaseID;
/// Cached collision data
void* mCachedCollisionData;
/// Pointer to user data
void* mUserData;
@ -85,13 +86,25 @@ class ProxyShape {
/// proxy shape will collide with every collision categories by default.
unsigned short mCollideWithMaskBits;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
// -------------------- Methods -------------------- //
/// Return the collision shape
CollisionShape* getCollisionShape();
public:
// -------------------- Methods -------------------- //
/// Constructor
ProxyShape(CollisionBody* body, CollisionShape* shape,
const Transform& transform, decimal mass);
const Transform& transform, decimal mass, MemoryManager& memoryManager);
/// Destructor
virtual ~ProxyShape();
@ -126,6 +139,12 @@ class ProxyShape {
/// Return the local to world transform
const Transform getLocalToWorldTransform() const;
/// Return the AABB of the proxy shape in world-space
const AABB getWorldAABB() const;
/// Test if the proxy shape overlaps with a given AABB
bool testAABBOverlap(const AABB& worldAABB) const;
/// Return true if a point is inside the collision shape
bool testPointInside(const Vector3& worldPoint);
@ -150,15 +169,19 @@ class ProxyShape {
/// Return the next proxy shape in the linked list of proxy shapes
const ProxyShape* getNext() const;
/// Return the pointer to the cached collision data
void** getCachedCollisionData();
/// Return the local scaling vector of the collision shape
Vector3 getLocalScaling() const;
/// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
void setProfiler(Profiler* profiler);
#endif
// -------------------- Friendship -------------------- //
friend class OverlappingPair;
@ -169,17 +192,13 @@ class ProxyShape {
friend class CollisionDetection;
friend class CollisionWorld;
friend class DynamicsWorld;
friend class EPAAlgorithm;
friend class GJKAlgorithm;
friend class ConvexMeshShape;
friend class ContactManifoldSet;
friend class MiddlePhaseTriangleCallback;
};
// Return the pointer to the cached collision data
inline void** ProxyShape::getCachedCollisionData() {
return &mCachedCollisionData;
}
// Return the collision shape
/**
* @return Pointer to the internal collision shape
@ -188,6 +207,14 @@ inline const CollisionShape* ProxyShape::getCollisionShape() const {
return mCollisionShape;
}
// Return the collision shape
/**
* @return Pointer to the internal collision shape
*/
inline CollisionShape* ProxyShape::getCollisionShape() {
return mCollisionShape;
}
// Return the parent body
/**
* @return Pointer to the parent body
@ -249,6 +276,16 @@ inline const Transform ProxyShape::getLocalToWorldTransform() const {
return mBody->mTransform * mLocalToBodyTransform;
}
// Return the AABB of the proxy shape in world-space
/**
* @return The AABB of the proxy shape in world-space
*/
inline const AABB ProxyShape::getWorldAABB() const {
AABB aabb;
mCollisionShape->computeAABB(aabb, getLocalToWorldTransform());
return aabb;
}
// Return the next proxy shape in the linked list of proxy shapes
/**
* @return Pointer to the next proxy shape in the linked list of proxy shapes
@ -302,7 +339,7 @@ inline void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBit
* @return The local scaling vector
*/
inline Vector3 ProxyShape::getLocalScaling() const {
return mCollisionShape->getScaling();
return mCollisionShape->getLocalScaling();
}
// Set the local scaling vector of the collision shape
@ -320,6 +357,27 @@ inline void ProxyShape::setLocalScaling(const Vector3& scaling) {
mBody->updateProxyShapeInBroadPhase(this, true);
}
/// Test if the proxy shape overlaps with a given AABB
/**
* @param worldAABB The AABB (in world-space coordinates) that will be used to test overlap
* @return True if the given AABB overlaps with the AABB of the collision body
*/
inline bool ProxyShape::testAABBOverlap(const AABB& worldAABB) const {
return worldAABB.testCollision(getWorldAABB());
}
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void ProxyShape::setProfiler(Profiler* profiler) {
mProfiler = profiler;
mCollisionShape->setProfiler(profiler);
}
#endif
}
#endif

2
src/collision/TriangleMesh.h
View File

@ -38,7 +38,7 @@ namespace reactphysics3d {
* This class represents a mesh made of triangles. A TriangleMesh contains
* one or several parts. Each part is a set of triangles represented in a
* TriangleVertexArray object describing all the triangles vertices of the part.
* A TriangleMesh object is used to create a ConcaveMeshShape from a triangle
* A TriangleMesh object can be used to create a ConcaveMeshShape from a triangle
* mesh for instance.
*/
class TriangleMesh {

240
src/collision/TriangleVertexArray.cpp
View File

@ -25,32 +25,258 @@
// Libraries
#include "TriangleVertexArray.h"
#include "mathematics/Vector3.h"
#include <cassert>
using namespace reactphysics3d;
// Constructor
// Constructor without vertices normals
/// Note that your data will not be copied into the TriangleVertexArray and
/// therefore, you need to make sure that those data are always valid during
/// the lifetime of the TriangleVertexArray.
/// the lifetime of the TriangleVertexArray. With this constructor, you do not
/// need to provide vertices normals for smooth mesh collision. Therefore, the
/// vertices normals will be computed automatically. The vertices normals are
/// computed with weighted average of the associated triangle face normal. The
/// weights are the angle between the associated edges of neighbor triangle face.
/**
* @param nbVertices Number of vertices in the array
* @param verticesStart Pointer to the first vertices of the array
* @param verticesStride Number of bytes between the beginning of two consecutive vertices
* @param nbTriangles Number of triangles in the array
* @param indexesStart Pointer to the first triangle index
* @param indexesStride Number of bytes between the beginning of the three indices of two triangles
* @param vertexDataType Type of data for the vertices (float, double)
* @param indexDataType Type of data for the indices (short, int)
*/
TriangleVertexArray::TriangleVertexArray(uint nbVertices, const void* verticesStart, uint verticesStride,
uint nbTriangles, const void* indexesStart, uint indexesStride,
VertexDataType vertexDataType, IndexDataType indexDataType) {
mNbVertices = nbVertices;
mVerticesStart = static_cast<const uchar*>(verticesStart);
mVerticesStride = verticesStride;
mVerticesNormalsStart = nullptr;
mVerticesNormalsStride = 3 * sizeof(float);
mNbTriangles = nbTriangles;
mIndicesStart = static_cast<const uchar*>(indexesStart);
mIndicesStride = indexesStride;
mVertexDataType = vertexDataType;
mVertexNormaldDataType = NormalDataType::NORMAL_FLOAT_TYPE;
mIndexDataType = indexDataType;
mAreVerticesNormalsProvidedByUser = false;
// Compute the vertices normals because they are not provided by the user
computeVerticesNormals();
}
// Constructor with vertices normals
/// Note that your data will not be copied into the TriangleVertexArray and
/// therefore, you need to make sure that those data are always valid during
/// the lifetime of the TriangleVertexArray. With this constructor, you need
/// to provide the vertices normals that will be used for smooth mesh collision.
/**
* @param nbVertices Number of vertices in the array
* @param verticesStart Pointer to the first vertices of the array
* @param verticesStride Number of bytes between the beginning of two consecutive vertices
* @param verticesNormalsStart Pointer to the first vertex normal of the array
* @param verticesNormalsStride Number of bytes between the beginning of two consecutive vertex normals
* @param nbTriangles Number of triangles in the array
* @param indexesStart Pointer to the first triangle index
* @param indexesStride Number of bytes between the beginning of two consecutive triangle indices
* @param vertexDataType Type of data for the vertices (float, double)
* @param indexDataType Type of data for the indices (short, int)
*/
TriangleVertexArray::TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
uint nbTriangles, void* indexesStart, int indexesStride,
VertexDataType vertexDataType, IndexDataType indexDataType) {
TriangleVertexArray::TriangleVertexArray(uint nbVertices, const void* verticesStart, uint verticesStride,
const void* verticesNormalsStart, uint verticesNormalsStride,
uint nbTriangles, const void* indexesStart, uint indexesStride,
VertexDataType vertexDataType, NormalDataType normalDataType,
IndexDataType indexDataType) {
mNbVertices = nbVertices;
mVerticesStart = reinterpret_cast<unsigned char*>(verticesStart);
mVerticesStart = static_cast<const uchar*>(verticesStart);
mVerticesStride = verticesStride;
mVerticesNormalsStart = static_cast<const uchar*>(verticesNormalsStart);
mVerticesNormalsStride = verticesNormalsStride;
mNbTriangles = nbTriangles;
mIndicesStart = reinterpret_cast<unsigned char*>(indexesStart);
mIndicesStart = static_cast<const uchar*>(indexesStart);
mIndicesStride = indexesStride;
mVertexDataType = vertexDataType;
mVertexNormaldDataType = normalDataType;
mIndexDataType = indexDataType;
mAreVerticesNormalsProvidedByUser = true;
assert(mVerticesNormalsStart != nullptr);
}
// Destructor
TriangleVertexArray::~TriangleVertexArray() {
// If the vertices normals have not been provided by the user
if (!mAreVerticesNormalsProvidedByUser) {
// Release the allocated memory
const void* verticesNormalPointer = static_cast<const void*>(mVerticesNormalsStart);
const float* verticesNormals = static_cast<const float*>(verticesNormalPointer);
delete[] verticesNormals;
}
}
// Compute the vertices normals when they are not provided by the user
/// The vertices normals are computed with weighted average of the associated
/// triangle face normal. The weights are the angle between the associated edges
/// of neighbor triangle face.
void TriangleVertexArray::computeVerticesNormals() {
// Allocate memory for the vertices normals
float* verticesNormals = new float[mNbVertices * 3];
// Init vertices normals to zero
for (uint i=0; i<mNbVertices * 3; i++) {
verticesNormals[i] = 0.0f;
}
// For each triangle face in the array
for (uint f=0; f < mNbTriangles; f++) {
// Get the indices of the three vertices of the triangle in the array
uint verticesIndices[3];
getTriangleVerticesIndices(f, verticesIndices);
// Get the triangle vertices
Vector3 triangleVertices[3];
getTriangleVertices(f, triangleVertices);
// Edges lengths
decimal edgesLengths[3];
edgesLengths[0] = (triangleVertices[1] - triangleVertices[0]).length();
edgesLengths[1] = (triangleVertices[2] - triangleVertices[1]).length();
edgesLengths[2] = (triangleVertices[0] - triangleVertices[2]).length();
// For each vertex of the face
for (uint v=0; v < 3; v++) {
uint previousVertex = (v == 0) ? 2 : v-1;
uint nextVertex = (v == 2) ? 0 : v+1;
Vector3 a = triangleVertices[nextVertex] - triangleVertices[v];
Vector3 b = triangleVertices[previousVertex] - triangleVertices[v];
Vector3 crossProduct = a.cross(b);
decimal sinA = crossProduct.length() / (edgesLengths[previousVertex] * edgesLengths[v]);
sinA = std::min(std::max(sinA, decimal(0.0)), decimal(1.0));
decimal arcSinA = std::asin(sinA);
assert(arcSinA >= decimal(0.0));
Vector3 normalComponent = arcSinA * crossProduct;
// Add the normal component of this vertex into the normals array
verticesNormals[verticesIndices[v] * 3] += normalComponent.x;
verticesNormals[verticesIndices[v] * 3 + 1] += normalComponent.y;
verticesNormals[verticesIndices[v] * 3 + 2] += normalComponent.z;
}
}
// Normalize the computed vertices normals
for (uint v=0; v<mNbVertices * 3; v += 3) {
// Normalize the normal
Vector3 normal(verticesNormals[v], verticesNormals[v + 1], verticesNormals[v + 2]);
normal.normalize();
verticesNormals[v] = normal.x;
verticesNormals[v + 1] = normal.y;
verticesNormals[v + 2] = normal.z;
}
const void* verticesNormalsPointer = static_cast<const void*>(verticesNormals);
mVerticesNormalsStart = static_cast<const uchar*>(verticesNormalsPointer);
}
// Return the indices of the three vertices of a given triangle in the array
void TriangleVertexArray::getTriangleVerticesIndices(uint triangleIndex, uint* outVerticesIndices) const {
assert(triangleIndex >= 0 && triangleIndex < mNbTriangles);
const uchar* triangleIndicesPointer = mIndicesStart + triangleIndex * mIndicesStride;
const void* startTriangleIndices = static_cast<const void*>(triangleIndicesPointer);
// For each vertex of the triangle
for (int i=0; i < 3; i++) {
// Get the index of the current vertex in the triangle
if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
outVerticesIndices[i] = static_cast<const uint*>(startTriangleIndices)[i];
}
else if (mIndexDataType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
outVerticesIndices[i] = static_cast<const ushort*>(startTriangleIndices)[i];
}
else {
assert(false);
}
}
}
// Return the vertices coordinates of a triangle
void TriangleVertexArray::getTriangleVertices(uint triangleIndex, Vector3* outTriangleVertices) const {
assert(triangleIndex >= 0 && triangleIndex < mNbTriangles);
// Get the three vertex index of the three vertices of the triangle
uint verticesIndices[3];
getTriangleVerticesIndices(triangleIndex, verticesIndices);
// For each vertex of the triangle
for (int k=0; k < 3; k++) {
const uchar* vertexPointerChar = mVerticesStart + verticesIndices[k] * mVerticesStride;
const void* vertexPointer = static_cast<const void*>(vertexPointerChar);
// Get the vertices components of the triangle
if (mVertexDataType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) {
const float* vertices = static_cast<const float*>(vertexPointer);
outTriangleVertices[k][0] = decimal(vertices[0]);
outTriangleVertices[k][1] = decimal(vertices[1]);
outTriangleVertices[k][2] = decimal(vertices[2]);
}
else if (mVertexDataType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) {
const double* vertices = static_cast<const double*>(vertexPointer);
outTriangleVertices[k][0] = decimal(vertices[0]);
outTriangleVertices[k][1] = decimal(vertices[1]);
outTriangleVertices[k][2] = decimal(vertices[2]);
}
else {
assert(false);
}
}
}
// Return the three vertices normals of a triangle
void TriangleVertexArray::getTriangleVerticesNormals(uint triangleIndex, Vector3* outTriangleVerticesNormals) const {
assert(triangleIndex >= 0 && triangleIndex < mNbTriangles);
// Get the three vertex index of the three vertices of the triangle
uint verticesIndices[3];
getTriangleVerticesIndices(triangleIndex, verticesIndices);
// For each vertex of the triangle
for (int k=0; k < 3; k++) {
const uchar* vertexNormalPointerChar = mVerticesNormalsStart + verticesIndices[k] * mVerticesNormalsStride;
const void* vertexNormalPointer = static_cast<const void*>(vertexNormalPointerChar);
// Get the normals from the array
if (mVertexNormaldDataType == TriangleVertexArray::NormalDataType::NORMAL_FLOAT_TYPE) {
const float* normal = static_cast<const float*>(vertexNormalPointer);
outTriangleVerticesNormals[k][0] = decimal(normal[0]);
outTriangleVerticesNormals[k][1] = decimal(normal[1]);
outTriangleVerticesNormals[k][2] = decimal(normal[2]);
}
else if (mVertexNormaldDataType == TriangleVertexArray::NormalDataType::NORMAL_DOUBLE_TYPE) {
const double* normal = static_cast<const double*>(vertexNormalPointer);
outTriangleVerticesNormals[k][0] = decimal(normal[0]);
outTriangleVerticesNormals[k][1] = decimal(normal[1]);
outTriangleVerticesNormals[k][2] = decimal(normal[2]);
}
else {
assert(false);
}
}
}

108
src/collision/TriangleVertexArray.h
View File

@ -31,6 +31,8 @@
namespace reactphysics3d {
struct Vector3;
// Class TriangleVertexArray
/**
* This class is used to describe the vertices and faces of a triangular mesh.
@ -48,50 +50,90 @@ class TriangleVertexArray {
/// Data type for the vertices in the array
enum class VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE};
/// Data type for the vertex normals in the array
enum class NormalDataType {NORMAL_FLOAT_TYPE, NORMAL_DOUBLE_TYPE};
/// Data type for the indices in the array
enum class IndexDataType {INDEX_INTEGER_TYPE, INDEX_SHORT_TYPE};
protected:
// -------------------- Attributes -------------------- //
/// Number of vertices in the array
uint mNbVertices;
/// Pointer to the first vertex value in the array
unsigned char* mVerticesStart;
const uchar* mVerticesStart;
/// Stride (number of bytes) between the beginning of two vertices
/// values in the array
int mVerticesStride;
uint mVerticesStride;
/// Pointer to the first vertex normal value in the array
const uchar* mVerticesNormalsStart;
/// Stride (number of bytes) between the beginning of two vertex normals
/// values in the array
uint mVerticesNormalsStride;
/// Number of triangles in the array
uint mNbTriangles;
/// Pointer to the first vertex index of the array
unsigned char* mIndicesStart;
const uchar* mIndicesStart;
/// Stride (number of bytes) between the beginning of two indices in
/// the array
int mIndicesStride;
/// Stride (number of bytes) between the beginning of the three indices of two triangles
uint mIndicesStride;
/// Data type of the vertices in the array
VertexDataType mVertexDataType;
/// Data type of the vertex normals in the array
NormalDataType mVertexNormaldDataType;
/// Data type of the indices in the array
IndexDataType mIndexDataType;
/// True if the vertices normals are provided by the user
bool mAreVerticesNormalsProvidedByUser;
// -------------------- Methods -------------------- //
/// Compute the vertices normals when they are not provided by the user
void computeVerticesNormals();
public:
/// Constructor
TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
uint nbTriangles, void* indexesStart, int indexesStride,
// -------------------- Methods -------------------- //
/// Constructor without vertices normals
TriangleVertexArray(uint nbVertices, const void* verticesStart, uint verticesStride,
uint nbTriangles, const void* indexesStart, uint indexesStride,
VertexDataType vertexDataType, IndexDataType indexDataType);
/// Constructor with vertices normals
TriangleVertexArray(uint nbVertices, const void* verticesStart, uint verticesStride,
const void* verticesNormalsStart, uint uverticesNormalsStride,
uint nbTriangles, const void* indexesStart, uint indexesStride,
VertexDataType vertexDataType, NormalDataType normalDataType,
IndexDataType indexDataType);
/// Destructor
~TriangleVertexArray() = default;
~TriangleVertexArray();
/// Deleted assignment operator
TriangleVertexArray& operator=(const TriangleVertexArray& triangleVertexArray) = delete;
/// Deleted copy-constructor
TriangleVertexArray(const TriangleVertexArray& triangleVertexArray) = delete;
/// Return the vertex data type
VertexDataType getVertexDataType() const;
/// Return the vertex normal data type
NormalDataType getVertexNormalDataType() const;
/// Return the index data type
IndexDataType getIndexDataType() const;
@ -102,16 +144,31 @@ class TriangleVertexArray {
uint getNbTriangles() const;
/// Return the vertices stride (number of bytes)
int getVerticesStride() const;
uint getVerticesStride() const;
/// Return the vertex normals stride (number of bytes)
uint getVerticesNormalsStride() const;
/// Return the indices stride (number of bytes)
int getIndicesStride() const;
uint getIndicesStride() const;
/// Return the pointer to the start of the vertices array
unsigned char* getVerticesStart() const;
const void* getVerticesStart() const;
/// Return the pointer to the start of the vertex normals array
const void* getVerticesNormalsStart() const;
/// Return the pointer to the start of the indices array
unsigned char* getIndicesStart() const;
const void* getIndicesStart() const;
/// Return the vertices coordinates of a triangle
void getTriangleVertices(uint triangleIndex, Vector3* outTriangleVertices) const;
/// Return the three vertices normals of a triangle
void getTriangleVerticesNormals(uint triangleIndex, Vector3* outTriangleVerticesNormals) const;
/// Return the indices of the three vertices of a given triangle in the array
void getTriangleVerticesIndices(uint triangleIndex, uint* outVerticesIndices) const;
};
// Return the vertex data type
@ -119,6 +176,11 @@ inline TriangleVertexArray::VertexDataType TriangleVertexArray::getVertexDataTyp
return mVertexDataType;
}
// Return the vertex normal data type
inline TriangleVertexArray::NormalDataType TriangleVertexArray::getVertexNormalDataType() const {
return mVertexNormaldDataType;
}
// Return the index data type
inline TriangleVertexArray::IndexDataType TriangleVertexArray::getIndexDataType() const {
return mIndexDataType;
@ -135,22 +197,32 @@ inline uint TriangleVertexArray::getNbTriangles() const {
}
// Return the vertices stride (number of bytes)
inline int TriangleVertexArray::getVerticesStride() const {
inline uint TriangleVertexArray::getVerticesStride() const {
return mVerticesStride;
}
// Return the vertex normals stride (number of bytes)
inline uint TriangleVertexArray::getVerticesNormalsStride() const {
return mVerticesNormalsStride;
}
// Return the indices stride (number of bytes)
inline int TriangleVertexArray::getIndicesStride() const {
inline uint TriangleVertexArray::getIndicesStride() const {
return mIndicesStride;
}
// Return the pointer to the start of the vertices array
inline unsigned char* TriangleVertexArray::getVerticesStart() const {
inline const void* TriangleVertexArray::getVerticesStart() const {
return mVerticesStart;
}
// Return the pointer to the start of the vertex normals array
inline const void* TriangleVertexArray::getVerticesNormalsStart() const {
return mVerticesNormalsStart;
}
// Return the pointer to the start of the indices array
inline unsigned char* TriangleVertexArray::getIndicesStart() const {
inline const void* TriangleVertexArray::getIndicesStart() const {
return mIndicesStart;
}

89
src/collision/broadphase/BroadPhaseAlgorithm.cpp
View File

@ -44,6 +44,13 @@ BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
// Allocate memory for the array of potential overlapping pairs
mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(mPotentialPairs != nullptr);
#ifdef IS_PROFILING_ACTIVE
mProfiler = nullptr;
#endif
}
// Destructor
@ -117,6 +124,8 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
// Add a proxy collision shape into the broad-phase collision detection
void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) {
assert(proxyShape->mBroadPhaseID == -1);
// Add the collision shape into the dynamic AABB tree and get its broad-phase ID
int nodeId = mDynamicAABBTree.addObject(aabb, proxyShape);
@ -131,8 +140,12 @@ void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const A
// Remove a proxy collision shape from the broad-phase collision detection
void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {
assert(proxyShape->mBroadPhaseID != -1);
int broadPhaseID = proxyShape->mBroadPhaseID;
proxyShape->mBroadPhaseID = -1;
// Remove the collision shape from the dynamic AABB tree
mDynamicAABBTree.removeObject(broadPhaseID);
@ -162,12 +175,25 @@ void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, cons
}
}
void BroadPhaseAlgorithm::reportAllShapesOverlappingWithAABB(const AABB& aabb,
LinkedList<int>& overlappingNodes) const {
AABBOverlapCallback callback(overlappingNodes);
// Ask the dynamic AABB tree to report all collision shapes that overlap with this AABB
mDynamicAABBTree.reportAllShapesOverlappingWithAABB(aabb, callback);
}
// Compute all the overlapping pairs of collision shapes
void BroadPhaseAlgorithm::computeOverlappingPairs() {
void BroadPhaseAlgorithm::computeOverlappingPairs(MemoryManager& memoryManager) {
// TODO : Try to see if we can allocate potential pairs in single frame allocator
// Reset the potential overlapping pairs
mNbPotentialPairs = 0;
LinkedList<int> overlappingNodes(memoryManager.getPoolAllocator());
// For all collision shapes that have moved (or have been created) during the
// last simulation step
for (uint i=0; i<mNbMovedShapes; i++) {
@ -175,7 +201,7 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
if (shapeID == -1) continue;
AABBOverlapCallback callback(*this, shapeID);
AABBOverlapCallback callback(overlappingNodes);
// Get the AABB of the shape
const AABB& shapeAABB = mDynamicAABBTree.getFatAABB(shapeID);
@ -184,6 +210,12 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
// this AABB. The method BroadPhase::notifiyOverlappingPair() will be called
// by the dynamic AABB tree for each potential overlapping pair.
mDynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, callback);
// Add the potential overlapping pairs
addOverlappingNodes(shapeID, overlappingNodes);
// Remove all the elements of the linked list of overlapping nodes
overlappingNodes.reset();
}
// Reset the array of collision shapes that have move (or have been created) during the
@ -208,8 +240,12 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
ProxyShape* shape1 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(pair->collisionShape1ID));
ProxyShape* shape2 = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(pair->collisionShape2ID));
// Notify the collision detection about the overlapping pair
mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);
// If the two proxy collision shapes are from the same body, skip it
if (shape1->getBody()->getID() != shape2->getBody()->getID()) {
// Notify the collision detection about the overlapping pair
mCollisionDetection.broadPhaseNotifyOverlappingPair(shape1, shape2);
}
// Skip the duplicate overlapping pairs
while (i < mNbPotentialPairs) {
@ -241,34 +277,41 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
}
// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
void BroadPhaseAlgorithm::notifyOverlappingNodes(int node1ID, int node2ID) {
void BroadPhaseAlgorithm::addOverlappingNodes(int referenceNodeId, const LinkedList<int>& overlappingNodes) {
// If both the nodes are the same, we do not create store the overlapping pair
if (node1ID == node2ID) return;
// For each overlapping node in the linked list
LinkedList<int>::ListElement* elem = overlappingNodes.getListHead();
while (elem != nullptr) {
// If we need to allocate more memory for the array of potential overlapping pairs
if (mNbPotentialPairs == mNbAllocatedPotentialPairs) {
// If both the nodes are the same, we do not create store the overlapping pair
if (referenceNodeId != elem->data) {
// Allocate more memory for the array of potential pairs
BroadPhasePair* oldPairs = mPotentialPairs;
mNbAllocatedPotentialPairs *= 2;
mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(mPotentialPairs);
memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair));
free(oldPairs);
// If we need to allocate more memory for the array of potential overlapping pairs
if (mNbPotentialPairs == mNbAllocatedPotentialPairs) {
// Allocate more memory for the array of potential pairs
BroadPhasePair* oldPairs = mPotentialPairs;
mNbAllocatedPotentialPairs *= 2;
mPotentialPairs = (BroadPhasePair*) malloc(mNbAllocatedPotentialPairs * sizeof(BroadPhasePair));
assert(mPotentialPairs);
memcpy(mPotentialPairs, oldPairs, mNbPotentialPairs * sizeof(BroadPhasePair));
free(oldPairs);
}
// Add the new potential pair into the array of potential overlapping pairs
mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(referenceNodeId, elem->data);
mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(referenceNodeId, elem->data);
mNbPotentialPairs++;
}
elem = elem->next;
}
// Add the new potential pair into the array of potential overlapping pairs
mPotentialPairs[mNbPotentialPairs].collisionShape1ID = std::min(node1ID, node2ID);
mPotentialPairs[mNbPotentialPairs].collisionShape2ID = std::max(node1ID, node2ID);
mNbPotentialPairs++;
}
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
void AABBOverlapCallback::notifyOverlappingNode(int nodeId) {
mBroadPhaseAlgorithm.notifyOverlappingNodes(mReferenceNodeId, nodeId);
mOverlappingNodes.insert(nodeId);
}
// Called for a broad-phase shape that has to be tested for raycast

69
src/collision/broadphase/BroadPhaseAlgorithm.h
View File

@ -32,6 +32,7 @@
#include "collision/ProxyShape.h"
#include "DynamicAABBTree.h"
#include "engine/Profiler.h"
#include "containers/LinkedList.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
@ -66,15 +67,13 @@ class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
private:
BroadPhaseAlgorithm& mBroadPhaseAlgorithm;
int mReferenceNodeId;
public:
LinkedList<int>& mOverlappingNodes;
// Constructor
AABBOverlapCallback(BroadPhaseAlgorithm& broadPhaseAlgo, int referenceNodeId)
: mBroadPhaseAlgorithm(broadPhaseAlgo), mReferenceNodeId(referenceNodeId) {
AABBOverlapCallback(LinkedList<int>& overlappingNodes)
: mOverlappingNodes(overlappingNodes) {
}
@ -163,6 +162,13 @@ class BroadPhaseAlgorithm {
/// Reference to the collision detection object
CollisionDetection& mCollisionDetection;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
public :
// -------------------- Methods -------------------- //
@ -197,18 +203,34 @@ class BroadPhaseAlgorithm {
/// step and that need to be tested again for broad-phase overlapping.
void removeMovedCollisionShape(int broadPhaseID);
/// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
void notifyOverlappingNodes(int broadPhaseId1, int broadPhaseId2);
/// Add potential overlapping pairs in the dynamic AABB tree
void addOverlappingNodes(int broadPhaseId1, const LinkedList<int>& overlappingNodes);
/// Report all the shapes that are overlapping with a given AABB
void reportAllShapesOverlappingWithAABB(const AABB& aabb, LinkedList<int>& overlappingNodes) const;
/// Compute all the overlapping pairs of collision shapes
void computeOverlappingPairs();
void computeOverlappingPairs(MemoryManager& memoryManager);
/// Return the proxy shape corresponding to the broad-phase node id in parameter
ProxyShape* getProxyShapeForBroadPhaseId(int broadPhaseId) const;
/// Return true if the two broad-phase collision shapes are overlapping
bool testOverlappingShapes(const ProxyShape* shape1, const ProxyShape* shape2) const;
/// Return the fat AABB of a given broad-phase shape
const AABB& getFatAABB(int broadPhaseId) const;
/// Ray casting method
void raycast(const Ray& ray, RaycastTest& raycastTest,
unsigned short raycastWithCategoryMaskBits) const;
void raycast(const Ray& ray, RaycastTest& raycastTest, unsigned short raycastWithCategoryMaskBits) const;
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
void setProfiler(Profiler* profiler);
#endif
};
// Method used to compare two pairs for sorting algorithm
@ -224,6 +246,9 @@ inline bool BroadPhasePair::smallerThan(const BroadPhasePair& pair1, const Broad
// Return true if the two broad-phase collision shapes are overlapping
inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1,
const ProxyShape* shape2) const {
if (shape1->mBroadPhaseID == -1 || shape2->mBroadPhaseID == -1) return false;
// Get the two AABBs of the collision shapes
const AABB& aabb1 = mDynamicAABBTree.getFatAABB(shape1->mBroadPhaseID);
const AABB& aabb2 = mDynamicAABBTree.getFatAABB(shape2->mBroadPhaseID);
@ -232,17 +257,37 @@ inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1,
return aabb1.testCollision(aabb2);
}
// Return the fat AABB of a given broad-phase shape
inline const AABB& BroadPhaseAlgorithm::getFatAABB(int broadPhaseId) const {
return mDynamicAABBTree.getFatAABB(broadPhaseId);
}
// Ray casting method
inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTest,
unsigned short raycastWithCategoryMaskBits) const {
PROFILE("BroadPhaseAlgorithm::raycast()");
PROFILE("BroadPhaseAlgorithm::raycast()", mProfiler);
BroadPhaseRaycastCallback broadPhaseRaycastCallback(mDynamicAABBTree, raycastWithCategoryMaskBits, raycastTest);
mDynamicAABBTree.raycast(ray, broadPhaseRaycastCallback);
}
// Return the proxy shape corresponding to the broad-phase node id in parameter
inline ProxyShape* BroadPhaseAlgorithm::getProxyShapeForBroadPhaseId(int broadPhaseId) const {
return static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(broadPhaseId));
}
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void BroadPhaseAlgorithm::setProfiler(Profiler* profiler) {
mProfiler = profiler;
mDynamicAABBTree.setProfiler(profiler);
}
#endif
}
#endif

6
src/collision/broadphase/DynamicAABBTree.cpp
View File

@ -26,7 +26,7 @@
// Libraries
#include "DynamicAABBTree.h"
#include "BroadPhaseAlgorithm.h"
#include "memory/Stack.h"
#include "containers/Stack.h"
#include "engine/Profiler.h"
using namespace reactphysics3d;
@ -171,7 +171,7 @@ void DynamicAABBTree::removeObject(int nodeID) {
/// (this can be useful if the shape AABB has become much smaller than the previous one for instance).
bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert) {
PROFILE("DynamicAABBTree::updateObject()");
PROFILE("DynamicAABBTree::updateObject()", mProfiler);
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf());
@ -633,7 +633,7 @@ void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb,
// Ray casting method
void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &callback) const {
PROFILE("DynamicAABBTree::raycast()");
PROFILE("DynamicAABBTree::raycast()", mProfiler);
decimal maxFraction = ray.maxFraction;

24
src/collision/broadphase/DynamicAABBTree.h
View File

@ -155,6 +155,13 @@ class DynamicAABBTree {
/// without triggering a large modification of the tree which can be costly
decimal mExtraAABBGap;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
// -------------------- Methods -------------------- //
/// Allocate and return a node to use in the tree
@ -237,6 +244,14 @@ class DynamicAABBTree {
/// Clear all the nodes and reset the tree
void reset();
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
void setProfiler(Profiler* profiler);
#endif
};
// Return true if the node is a leaf of the tree
@ -292,6 +307,15 @@ inline int DynamicAABBTree::addObject(const AABB& aabb, void* data) {
return nodeId;
}
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void DynamicAABBTree::setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
#endif
}
#endif

227
src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.cpp Executable file
View File

@ -0,0 +1,227 @@
/********************************************************************************
* 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 "CapsuleVsCapsuleAlgorithm.h"
#include "collision/shapes/CapsuleShape.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Compute the narrow-phase collision detection between two capsules
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool CapsuleVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
MemoryAllocator& memoryAllocator) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
// Get the capsule collision shapes
const CapsuleShape* capsuleShape1 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape1);
const CapsuleShape* capsuleShape2 = static_cast<const CapsuleShape*>(narrowPhaseInfo->collisionShape2);
// Get the transform from capsule 1 local-space to capsule 2 local-space
const Transform capsule1ToCapsule2SpaceTransform = narrowPhaseInfo->shape2ToWorldTransform.getInverse() * narrowPhaseInfo->shape1ToWorldTransform;
// Compute the end-points of the inner segment of the first capsule
Vector3 capsule1SegA(0, -capsuleShape1->getHeight() * decimal(0.5), 0);
Vector3 capsule1SegB(0, capsuleShape1->getHeight() * decimal(0.5), 0);
capsule1SegA = capsule1ToCapsule2SpaceTransform * capsule1SegA;
capsule1SegB = capsule1ToCapsule2SpaceTransform * capsule1SegB;
// Compute the end-points of the inner segment of the second capsule
const Vector3 capsule2SegA(0, -capsuleShape2->getHeight() * decimal(0.5), 0);
const Vector3 capsule2SegB(0, capsuleShape2->getHeight() * decimal(0.5), 0);
// The two inner capsule segments
const Vector3 seg1 = capsule1SegB - capsule1SegA;
const Vector3 seg2 = capsule2SegB - capsule2SegA;
// Compute the sum of the radius of the two capsules (virtual spheres)
decimal sumRadius = capsuleShape2->getRadius() + capsuleShape1->getRadius();
// If the two capsules are parallel (we create two contact points)
bool areCapsuleInnerSegmentsParralel = areParallelVectors(seg1, seg2);
if (areCapsuleInnerSegmentsParralel) {
// If the distance between the two segments is larger than the sum of the capsules radius (we do not have overlapping)
const decimal segmentsPerpendicularDistance = computePointToLineDistance(capsule1SegA, capsule1SegB, capsule2SegA);
if (segmentsPerpendicularDistance >= sumRadius) {
// The capsule are parallel but their inner segment distance is larger than the sum of the capsules radius.
// Therefore, we do not have overlap. If the inner segments overlap, we do not report any collision.
return false;
}
// Compute the planes that goes through the extreme points of the inner segment of capsule 1
decimal d1 = seg1.dot(capsule1SegA);
decimal d2 = -seg1.dot(capsule1SegB);
// Clip the inner segment of capsule 2 with the two planes that go through extreme points of inner
// segment of capsule 1
decimal t1 = computePlaneSegmentIntersection(capsule2SegB, capsule2SegA, d1, seg1);
decimal t2 = computePlaneSegmentIntersection(capsule2SegA, capsule2SegB, d2, -seg1);
// If the segments were overlapping (the clip segment is valid)
if (t1 > decimal(0.0) && t2 > decimal(0.0)) {
if (reportContacts) {
// Clip the inner segment of capsule 2
if (t1 > decimal(1.0)) t1 = decimal(1.0);
const Vector3 clipPointA = capsule2SegB - t1 * seg2;
if (t2 > decimal(1.0)) t2 = decimal(1.0);
const Vector3 clipPointB = capsule2SegA + t2 * seg2;
// Project point capsule2SegA onto line of innner segment of capsule 1
const Vector3 seg1Normalized = seg1.getUnit();
Vector3 pointOnInnerSegCapsule1 = capsule1SegA + seg1Normalized.dot(capsule2SegA - capsule1SegA) * seg1Normalized;
Vector3 normalCapsule2SpaceNormalized;
Vector3 segment1ToSegment2;
// If the inner capsule segments perpendicular distance is not zero (the inner segments are not overlapping)
if (segmentsPerpendicularDistance > MACHINE_EPSILON) {
// Compute a perpendicular vector from segment 1 to segment 2
segment1ToSegment2 = (capsule2SegA - pointOnInnerSegCapsule1);
normalCapsule2SpaceNormalized = segment1ToSegment2.getUnit();
}
else { // If the capsule inner segments are overlapping (degenerate case)
// We cannot use the vector between segments as a contact normal. To generate a contact normal, we take
// any vector that is orthogonal to the inner capsule segments.
Vector3 vec1(1, 0, 0);
Vector3 vec2(0, 1, 0);
Vector3 seg2Normalized = seg2.getUnit();
// Get the vectors (among vec1 and vec2) that is the most orthogonal to the capsule 2 inner segment (smallest absolute dot product)
decimal cosA1 = std::abs(seg2Normalized.x); // abs(vec1.dot(seg2))
decimal cosA2 = std::abs(seg2Normalized.y); // abs(vec2.dot(seg2))
segment1ToSegment2.setToZero();
// We choose as a contact normal, any direction that is perpendicular to the inner capsules segments
normalCapsule2SpaceNormalized = cosA1 < cosA2 ? seg2Normalized.cross(vec1) : seg2Normalized.cross(vec2);
}
Transform capsule2ToCapsule1SpaceTransform = capsule1ToCapsule2SpaceTransform.getInverse();
const Vector3 contactPointACapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointA - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsuleShape1->getRadius());
const Vector3 contactPointBCapsule1Local = capsule2ToCapsule1SpaceTransform * (clipPointB - segment1ToSegment2 + normalCapsule2SpaceNormalized * capsuleShape1->getRadius());
const Vector3 contactPointACapsule2Local = clipPointA - normalCapsule2SpaceNormalized * capsuleShape2->getRadius();
const Vector3 contactPointBCapsule2Local = clipPointB - normalCapsule2SpaceNormalized * capsuleShape2->getRadius();
decimal penetrationDepth = sumRadius - segmentsPerpendicularDistance;
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsule2SpaceNormalized;
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointACapsule1Local, contactPointACapsule2Local);
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointBCapsule1Local, contactPointBCapsule2Local);
}
return true;
}
}
// Compute the closest points between the two inner capsule segments
Vector3 closestPointCapsule1Seg;
Vector3 closestPointCapsule2Seg;
computeClosestPointBetweenTwoSegments(capsule1SegA, capsule1SegB, capsule2SegA, capsule2SegB,
closestPointCapsule1Seg, closestPointCapsule2Seg);
// Compute the distance between the sphere center and the closest point on the segment
Vector3 closestPointsSeg1ToSeg2 = (closestPointCapsule2Seg - closestPointCapsule1Seg);
const decimal closestPointsDistanceSquare = closestPointsSeg1ToSeg2.lengthSquare();
// If the collision shapes overlap
if (closestPointsDistanceSquare < sumRadius * sumRadius) {
if (reportContacts) {
// If the distance between the inner segments is not zero
if (closestPointsDistanceSquare > MACHINE_EPSILON) {
decimal closestPointsDistance = std::sqrt(closestPointsDistanceSquare);
closestPointsSeg1ToSeg2 /= closestPointsDistance;
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + closestPointsSeg1ToSeg2 * capsuleShape1->getRadius());
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - closestPointsSeg1ToSeg2 * capsuleShape2->getRadius();
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * closestPointsSeg1ToSeg2;
decimal penetrationDepth = sumRadius - closestPointsDistance;
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth, contactPointCapsule1Local, contactPointCapsule2Local);
}
else { // The segment are overlapping (degenerate case)
// If the capsule segments are parralel
if (areCapsuleInnerSegmentsParralel) {
// The segment are parallel, not overlapping and their distance is zero.
// Therefore, the capsules are just touching at the top of their inner segments
decimal squareDistCapsule2PointToCapsuleSegA = (capsule1SegA - closestPointCapsule2Seg).lengthSquare();
Vector3 capsule1SegmentMostExtremePoint = squareDistCapsule2PointToCapsuleSegA > MACHINE_EPSILON ? capsule1SegA : capsule1SegB;
Vector3 normalCapsuleSpace2 = (closestPointCapsule2Seg - capsule1SegmentMostExtremePoint);
normalCapsuleSpace2.normalize();
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius());
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius();
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2;
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local);
}
else { // If the capsules inner segments are not parallel
// We cannot use a vector between the segments as contact normal. We need to compute a new contact normal with the cross
// product between the two segments.
Vector3 normalCapsuleSpace2 = seg1.cross(seg2);
normalCapsuleSpace2.normalize();
// Compute the contact points on both shapes
const Vector3 contactPointCapsule1Local = capsule1ToCapsule2SpaceTransform.getInverse() * (closestPointCapsule1Seg + normalCapsuleSpace2 * capsuleShape1->getRadius());
const Vector3 contactPointCapsule2Local = closestPointCapsule2Seg - normalCapsuleSpace2 * capsuleShape2->getRadius();
const Vector3 normalWorld = narrowPhaseInfo->shape2ToWorldTransform.getOrientation() * normalCapsuleSpace2;
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, sumRadius, contactPointCapsule1Local, contactPointCapsule2Local);
}
}
}
return true;
}
return false;
}

145
src/collision/CollisionShapeInfo.h → src/collision/narrowphase/CapsuleVsCapsuleAlgorithm.h
View File

@ -1,75 +1,70 @@
/********************************************************************************
* 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_COLLISION_SHAPE_INFO_H
#define REACTPHYSICS3D_COLLISION_SHAPE_INFO_H
// Libraries
#include "shapes/CollisionShape.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
class OverlappingPair;
// Class CollisionShapeInfo
/**
* This structure regroups different things about a collision shape. This is
* used to pass information about a collision shape to a collision algorithm.
*/
struct CollisionShapeInfo {
public:
/// Broadphase overlapping pair
OverlappingPair* overlappingPair;
/// Proxy shape
ProxyShape* proxyShape;
/// Pointer to the collision shape
const CollisionShape* collisionShape;
/// Transform that maps from collision shape local-space to world-space
Transform shapeToWorldTransform;
/// Cached collision data of the proxy shape
void** cachedCollisionData;
/// Constructor
CollisionShapeInfo(ProxyShape* proxyCollisionShape, const CollisionShape* shape,
const Transform& shapeLocalToWorldTransform, OverlappingPair* pair,
void** cachedData)
: overlappingPair(pair), proxyShape(proxyCollisionShape), collisionShape(shape),
shapeToWorldTransform(shapeLocalToWorldTransform),
cachedCollisionData(cachedData) {
}
};
}
#endif
/********************************************************************************
* 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_CAPSULE_VS_CAPSULE_ALGORITHM_H
#define REACTPHYSICS3D_CAPSULE_VS_CAPSULE_ALGORITHM_H
// Libraries
#include "body/Body.h"
#include "constraint/ContactPoint.h"
#include "NarrowPhaseAlgorithm.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Class CapsuleVsCapsuleAlgorithm
/**
* This class is used to compute the narrow-phase collision detection
* between two capsules collision shapes.
*/
class CapsuleVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
protected :
public :
// -------------------- Methods -------------------- //
/// Constructor
CapsuleVsCapsuleAlgorithm() = default;
/// Destructor
virtual ~CapsuleVsCapsuleAlgorithm() override = default;
/// Deleted copy-constructor
CapsuleVsCapsuleAlgorithm(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
/// Deleted assignment operator
CapsuleVsCapsuleAlgorithm& operator=(const CapsuleVsCapsuleAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between two capsules
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override;
};
}
#endif

164
src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.cpp Normal file
View File

@ -0,0 +1,164 @@
/********************************************************************************
* 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 "CapsuleVsConvexPolyhedronAlgorithm.h"
#include "SAT/SATAlgorithm.h"
#include "GJK/GJKAlgorithm.h"
#include "collision/shapes/CapsuleShape.h"
#include "collision/shapes/ConvexPolyhedronShape.h"
#include <cassert>
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Compute the narrow-phase collision detection between a capsule and a polyhedron
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool CapsuleVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
MemoryAllocator& memoryAllocator) {
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
SATAlgorithm satAlgorithm(memoryAllocator);
#ifdef IS_PROFILING_ACTIVE
gjkAlgorithm.setProfiler(mProfiler);
satAlgorithm.setProfiler(mProfiler);
#endif
// Get the last frame collision info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts);
lastFrameCollisionInfo->wasUsingGJK = true;
lastFrameCollisionInfo->wasUsingSAT = false;
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CAPSULE);
// If we have found a contact point inside the margins (shallow penetration)
if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
if (reportContacts) {
// GJK has found a shallow contact. If the face of the polyhedron mesh is orthogonal to the
// capsule inner segment and parallel to the contact point normal, we would like to create
// two contact points instead of a single one (as in the deep contact case with SAT algorithm)
// Get the contact point created by GJK
ContactPointInfo* contactPoint = narrowPhaseInfo->contactPoints;
assert(contactPoint != nullptr);
bool isCapsuleShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE;
// Get the collision shapes
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
const ConvexPolyhedronShape* polyhedron = static_cast<const ConvexPolyhedronShape*>(isCapsuleShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1);
// For each face of the polyhedron
for (uint f = 0; f < polyhedron->getNbFaces(); f++) {
const Transform polyhedronToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform;
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
// Get the face normal
const Vector3 faceNormal = polyhedron->getFaceNormal(f);
Vector3 faceNormalWorld = polyhedronToWorld.getOrientation() * faceNormal;
const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
Vector3 capsuleInnerSegmentDirection = capsuleToWorld.getOrientation() * (capsuleSegB - capsuleSegA);
capsuleInnerSegmentDirection.normalize();
bool isFaceNormalInDirectionOfContactNormal = faceNormalWorld.dot(contactPoint->normal) > decimal(0.0);
bool isFaceNormalInContactDirection = (isCapsuleShape1 && !isFaceNormalInDirectionOfContactNormal) || (!isCapsuleShape1 && isFaceNormalInDirectionOfContactNormal);
// If the polyhedron face normal is orthogonal to the capsule inner segment and parallel to the contact point normal and the face normal
// is in direction of the contact normal (from the polyhedron point of view).
if (isFaceNormalInContactDirection && areOrthogonalVectors(faceNormalWorld, capsuleInnerSegmentDirection)
&& areParallelVectors(faceNormalWorld, contactPoint->normal)) {
// Remove the previous contact point computed by GJK
narrowPhaseInfo->resetContactPoints();
const Transform capsuleToWorld = isCapsuleShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
const Transform polyhedronToCapsuleTransform = capsuleToWorld.getInverse() * polyhedronToWorld;
// Compute the end-points of the inner segment of the capsule
const Vector3 capsuleSegA(0, -capsuleShape->getHeight() * decimal(0.5), 0);
const Vector3 capsuleSegB(0, capsuleShape->getHeight() * decimal(0.5), 0);
// Convert the inner capsule segment points into the polyhedron local-space
const Transform capsuleToPolyhedronTransform = polyhedronToCapsuleTransform.getInverse();
const Vector3 capsuleSegAPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegA;
const Vector3 capsuleSegBPolyhedronSpace = capsuleToPolyhedronTransform * capsuleSegB;
const Vector3 separatingAxisCapsuleSpace = polyhedronToCapsuleTransform.getOrientation() * faceNormal;
if (isCapsuleShape1) {
faceNormalWorld = -faceNormalWorld;
}
// Compute and create two contact points
bool contactsFound = satAlgorithm.computeCapsulePolyhedronFaceContactPoints(f, capsuleShape->getRadius(), polyhedron, contactPoint->penetrationDepth,
polyhedronToCapsuleTransform, faceNormalWorld, separatingAxisCapsuleSpace,
capsuleSegAPolyhedronSpace, capsuleSegBPolyhedronSpace,
narrowPhaseInfo, isCapsuleShape1);
if (!contactsFound) {
return false;
}
break;
}
}
}
lastFrameCollisionInfo->wasUsingSAT = false;
lastFrameCollisionInfo->wasUsingGJK = false;
// Return true
return true;
}
// If we have overlap even without the margins (deep penetration)
if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) {
// Run the SAT algorithm to find the separating axis and compute contact point
bool isColliding = satAlgorithm.testCollisionCapsuleVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
lastFrameCollisionInfo->wasUsingGJK = false;
lastFrameCollisionInfo->wasUsingSAT = true;
return isColliding;
}
return false;
}

70
src/collision/narrowphase/CapsuleVsConvexPolyhedronAlgorithm.h Normal file
View File

@ -0,0 +1,70 @@
/********************************************************************************
* 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_CAPSULE_VS_CONVEX_POLYHEDRON_ALGORITHM_H
#define REACTPHYSICS3D_CAPSULE_VS_CONVEX_POLYHEDRON_ALGORITHM_H
// Libraries
#include "body/Body.h"
#include "constraint/ContactPoint.h"
#include "NarrowPhaseAlgorithm.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Class CapsuleVsConvexPolyhedronAlgorithm
/**
* This class is used to compute the narrow-phase collision detection
* between a capsule and a convex polyhedron.
*/
class CapsuleVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
protected :
public :
// -------------------- Methods -------------------- //
/// Constructor
CapsuleVsConvexPolyhedronAlgorithm() = default;
/// Destructor
virtual ~CapsuleVsConvexPolyhedronAlgorithm() override = default;
/// Deleted copy-constructor
CapsuleVsConvexPolyhedronAlgorithm(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Deleted assignment operator
CapsuleVsConvexPolyhedronAlgorithm& operator=(const CapsuleVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between a capsule and a polyhedron
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override;
};
}
#endif

34
src/collision/narrowphase/CollisionDispatch.h
View File

@ -41,6 +41,13 @@ class CollisionDispatch {
protected:
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
public:
/// Constructor
@ -49,18 +56,29 @@ class CollisionDispatch {
/// Destructor
virtual ~CollisionDispatch() = default;
/// Initialize the collision dispatch configuration
virtual void init(CollisionDetection* collisionDetection,
MemoryAllocator* memoryAllocator) {
}
/// Select and return the narrow-phase collision detection algorithm to
/// use between two types of collision shapes.
virtual NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type,
int shape2Type)=0;
virtual NarrowPhaseAlgorithm* selectAlgorithm(int shape1Type, int shape2Type)=0;
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
virtual void setProfiler(Profiler* profiler);
#endif
};
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void CollisionDispatch::setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
#endif
}
#endif

281
src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp
View File

@ -1,281 +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 "collision/shapes/ConcaveShape.h"
#include "collision/shapes/TriangleShape.h"
#include "ConcaveVsConvexAlgorithm.h"
#include "collision/CollisionDetection.h"
#include "engine/CollisionWorld.h"
#include <algorithm>
using namespace reactphysics3d;
// Return true and compute a contact info if the two bounding volumes collide
void ConcaveVsConvexAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) {
ProxyShape* convexProxyShape;
ProxyShape* concaveProxyShape;
const ConvexShape* convexShape;
const ConcaveShape* concaveShape;
// Collision shape 1 is convex, collision shape 2 is concave
if (shape1Info.collisionShape->isConvex()) {
convexProxyShape = shape1Info.proxyShape;
convexShape = static_cast<const ConvexShape*>(shape1Info.collisionShape);
concaveProxyShape = shape2Info.proxyShape;
concaveShape = static_cast<const ConcaveShape*>(shape2Info.collisionShape);
}
else { // Collision shape 2 is convex, collision shape 1 is concave
convexProxyShape = shape2Info.proxyShape;
convexShape = static_cast<const ConvexShape*>(shape2Info.collisionShape);
concaveProxyShape = shape1Info.proxyShape;
concaveShape = static_cast<const ConcaveShape*>(shape1Info.collisionShape);
}
// Set the parameters of the callback object
ConvexVsTriangleCallback convexVsTriangleCallback;
convexVsTriangleCallback.setCollisionDetection(mCollisionDetection);
convexVsTriangleCallback.setConvexShape(convexShape);
convexVsTriangleCallback.setConcaveShape(concaveShape);
convexVsTriangleCallback.setProxyShapes(convexProxyShape, concaveProxyShape);
convexVsTriangleCallback.setOverlappingPair(shape1Info.overlappingPair);
// Compute the convex shape AABB in the local-space of the convex shape
AABB aabb;
convexShape->computeAABB(aabb, convexProxyShape->getLocalToWorldTransform());
// If smooth mesh collision is enabled for the concave mesh
if (concaveShape->getIsSmoothMeshCollisionEnabled()) {
std::vector<SmoothMeshContactInfo> contactPoints;
SmoothCollisionNarrowPhaseCallback smoothNarrowPhaseCallback(contactPoints);
convexVsTriangleCallback.setNarrowPhaseCallback(&smoothNarrowPhaseCallback);
// Call the convex vs triangle callback for each triangle of the concave shape
concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
// Run the smooth mesh collision algorithm
processSmoothMeshCollision(shape1Info.overlappingPair, contactPoints, narrowPhaseCallback);
}
else {
convexVsTriangleCallback.setNarrowPhaseCallback(narrowPhaseCallback);
// Call the convex vs triangle callback for each triangle of the concave shape
concaveShape->testAllTriangles(convexVsTriangleCallback, aabb);
}
}
// Test collision between a triangle and the convex mesh shape
void ConvexVsTriangleCallback::testTriangle(const Vector3* trianglePoints) {
// Create a triangle collision shape
decimal margin = mConcaveShape->getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
// Select the collision algorithm to use between the triangle and the convex shape
NarrowPhaseAlgorithm* algo = mCollisionDetection->getCollisionAlgorithm(triangleShape.getType(),
mConvexShape->getType());
// If there is no collision algorithm between those two kinds of shapes
if (algo == nullptr) return;
// Notify the narrow-phase algorithm about the overlapping pair we are going to test
algo->setCurrentOverlappingPair(mOverlappingPair);
// Create the CollisionShapeInfo objects
CollisionShapeInfo shapeConvexInfo(mConvexProxyShape, mConvexShape, mConvexProxyShape->getLocalToWorldTransform(),
mOverlappingPair, mConvexProxyShape->getCachedCollisionData());
CollisionShapeInfo shapeConcaveInfo(mConcaveProxyShape, &triangleShape,
mConcaveProxyShape->getLocalToWorldTransform(),
mOverlappingPair, mConcaveProxyShape->getCachedCollisionData());
// Use the collision algorithm to test collision between the triangle and the other convex shape
algo->testCollision(shapeConvexInfo, shapeConcaveInfo, mNarrowPhaseCallback);
}
// Process the concave triangle mesh collision using the smooth mesh collision algorithm described
// by Pierre Terdiman (http://www.codercorner.com/MeshContacts.pdf). This is used to avoid the collision
// issue with some internal edges.
void ConcaveVsConvexAlgorithm::processSmoothMeshCollision(OverlappingPair* overlappingPair,
std::vector<SmoothMeshContactInfo> contactPoints,
NarrowPhaseCallback* narrowPhaseCallback) {
// Set with the triangle vertices already processed to void further contacts with same triangle
std::unordered_multimap<int, Vector3> processTriangleVertices;
// Sort the list of narrow-phase contacts according to their penetration depth
std::sort(contactPoints.begin(), contactPoints.end(), ContactsDepthCompare());
// For each contact point (from smaller penetration depth to larger)
std::vector<SmoothMeshContactInfo>::const_iterator it;
for (it = contactPoints.begin(); it != contactPoints.end(); ++it) {
const SmoothMeshContactInfo info = *it;
const Vector3& contactPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
// Compute the barycentric coordinates of the point in the triangle
decimal u, v, w;
computeBarycentricCoordinatesInTriangle(info.triangleVertices[0],
info.triangleVertices[1],
info.triangleVertices[2],
contactPoint, u, v, w);
int nbZeros = 0;
bool isUZero = approxEqual(u, 0, 0.0001);
bool isVZero = approxEqual(v, 0, 0.0001);
bool isWZero = approxEqual(w, 0, 0.0001);
if (isUZero) nbZeros++;
if (isVZero) nbZeros++;
if (isWZero) nbZeros++;
// If it is a vertex contact
if (nbZeros == 2) {
Vector3 contactVertex = !isUZero ? info.triangleVertices[0] : (!isVZero ? info.triangleVertices[1] : info.triangleVertices[2]);
// Check that this triangle vertex has not been processed yet
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex)) {
// Keep the contact as it is and report it
narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
}
}
else if (nbZeros == 1) { // If it is an edge contact
Vector3 contactVertex1 = isUZero ? info.triangleVertices[1] : (isVZero ? info.triangleVertices[0] : info.triangleVertices[0]);
Vector3 contactVertex2 = isUZero ? info.triangleVertices[2] : (isVZero ? info.triangleVertices[2] : info.triangleVertices[1]);
// Check that this triangle edge has not been processed yet
if (!hasVertexBeenProcessed(processTriangleVertices, contactVertex1) &&
!hasVertexBeenProcessed(processTriangleVertices, contactVertex2)) {
// Keep the contact as it is and report it
narrowPhaseCallback->notifyContact(overlappingPair, info.contactInfo);
}
}
else { // If it is a face contact
ContactPointInfo newContactInfo(info.contactInfo);
ProxyShape* firstShape;
ProxyShape* secondShape;
if (info.isFirstShapeTriangle) {
firstShape = overlappingPair->getShape1();
secondShape = overlappingPair->getShape2();
}
else {
firstShape = overlappingPair->getShape2();
secondShape = overlappingPair->getShape1();
}
// We use the triangle normal as the contact normal
Vector3 a = info.triangleVertices[1] - info.triangleVertices[0];
Vector3 b = info.triangleVertices[2] - info.triangleVertices[0];
Vector3 localNormal = a.cross(b);
newContactInfo.normal = firstShape->getLocalToWorldTransform().getOrientation() * localNormal;
Vector3 firstLocalPoint = info.isFirstShapeTriangle ? info.contactInfo.localPoint1 : info.contactInfo.localPoint2;
Vector3 firstWorldPoint = firstShape->getLocalToWorldTransform() * firstLocalPoint;
newContactInfo.normal.normalize();
if (newContactInfo.normal.dot(info.contactInfo.normal) < 0) {
newContactInfo.normal = -newContactInfo.normal;
}
// We recompute the contact point on the second body with the new normal as described in
// the Smooth Mesh Contacts with GJK of the Game Physics Pearls book (from Gino van Den Bergen and
// Dirk Gregorius) to avoid adding torque
Transform worldToLocalSecondPoint = secondShape->getLocalToWorldTransform().getInverse();
if (info.isFirstShapeTriangle) {
Vector3 newSecondWorldPoint = firstWorldPoint + newContactInfo.normal;
newContactInfo.localPoint2 = worldToLocalSecondPoint * newSecondWorldPoint;
}
else {
Vector3 newSecondWorldPoint = firstWorldPoint - newContactInfo.normal;
newContactInfo.localPoint1 = worldToLocalSecondPoint * newSecondWorldPoint;
}
// Report the contact
narrowPhaseCallback->notifyContact(overlappingPair, newContactInfo);
}
// Add the three vertices of the triangle to the set of processed
// triangle vertices
addProcessedVertex(processTriangleVertices, info.triangleVertices[0]);
addProcessedVertex(processTriangleVertices, info.triangleVertices[1]);
addProcessedVertex(processTriangleVertices, info.triangleVertices[2]);
}
}
// Return true if the vertex is in the set of already processed vertices
bool ConcaveVsConvexAlgorithm::hasVertexBeenProcessed(const std::unordered_multimap<int, Vector3>& processTriangleVertices, const Vector3& vertex) const {
int key = int(vertex.x * vertex.y * vertex.z);
auto range = processTriangleVertices.equal_range(key);
for (auto it = range.first; it != range.second; ++it) {
if (vertex.x == it->second.x && vertex.y == it->second.y && vertex.z == it->second.z) return true;
}
return false;
}
// Called by a narrow-phase collision algorithm when a new contact has been found
void SmoothCollisionNarrowPhaseCallback::notifyContact(OverlappingPair* overlappingPair,
const ContactPointInfo& contactInfo) {
Vector3 triangleVertices[3];
bool isFirstShapeTriangle;
// If the collision shape 1 is the triangle
if (contactInfo.collisionShape1->getType() == CollisionShapeType::TRIANGLE) {
assert(contactInfo.collisionShape2->getType() != CollisionShapeType::TRIANGLE);
const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape1);
triangleVertices[0] = triangleShape->getVertex(0);
triangleVertices[1] = triangleShape->getVertex(1);
triangleVertices[2] = triangleShape->getVertex(2);
isFirstShapeTriangle = true;
}
else { // If the collision shape 2 is the triangle
assert(contactInfo.collisionShape2->getType() == CollisionShapeType::TRIANGLE);
const TriangleShape* triangleShape = static_cast<const TriangleShape*>(contactInfo.collisionShape2);
triangleVertices[0] = triangleShape->getVertex(0);
triangleVertices[1] = triangleShape->getVertex(1);
triangleVertices[2] = triangleShape->getVertex(2);
isFirstShapeTriangle = false;
}
SmoothMeshContactInfo smoothContactInfo(contactInfo, isFirstShapeTriangle, triangleVertices[0], triangleVertices[1], triangleVertices[2]);
// Add the narrow-phase contact into the list of contact to process for
// smooth mesh collision
mContactPoints.push_back(smoothContactInfo);
}

237
src/collision/narrowphase/ConcaveVsConvexAlgorithm.h
View File

@ -1,237 +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_CONCAVE_VS_CONVEX_ALGORITHM_H
#define REACTPHYSICS3D_CONCAVE_VS_CONVEX_ALGORITHM_H
// Libraries
#include "NarrowPhaseAlgorithm.h"
#include "collision/shapes/ConvexShape.h"
#include "collision/shapes/ConcaveShape.h"
#include <unordered_map>
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Class ConvexVsTriangleCallback
/**
* This class is used to encapsulate a callback method for
* collision detection between the triangle of a concave mesh shape
* and a convex shape.
*/
class ConvexVsTriangleCallback : public TriangleCallback {
protected:
/// Pointer to the collision detection object
CollisionDetection* mCollisionDetection;
/// Narrow-phase collision callback
NarrowPhaseCallback* mNarrowPhaseCallback;
/// Convex collision shape to test collision with
const ConvexShape* mConvexShape;
/// Concave collision shape
const ConcaveShape* mConcaveShape;
/// Proxy shape of the convex collision shape
ProxyShape* mConvexProxyShape;
/// Proxy shape of the concave collision shape
ProxyShape* mConcaveProxyShape;
/// Broadphase overlapping pair
OverlappingPair* mOverlappingPair;
/// Used to sort ContactPointInfos according to their penetration depth
static bool contactsDepthCompare(const ContactPointInfo& contact1,
const ContactPointInfo& contact2);
public:
/// Destructor
virtual ~ConvexVsTriangleCallback() override = default;
/// Set the collision detection pointer
void setCollisionDetection(CollisionDetection* collisionDetection) {
mCollisionDetection = collisionDetection;
}
/// Set the narrow-phase collision callback
void setNarrowPhaseCallback(NarrowPhaseCallback* callback) {
mNarrowPhaseCallback = callback;
}
/// Set the convex collision shape to test collision with
void setConvexShape(const ConvexShape* convexShape) {
mConvexShape = convexShape;
}
/// Set the concave collision shape
void setConcaveShape(const ConcaveShape* concaveShape) {
mConcaveShape = concaveShape;
}
/// Set the broadphase overlapping pair
void setOverlappingPair(OverlappingPair* overlappingPair) {
mOverlappingPair = overlappingPair;
}
/// Set the proxy shapes of the two collision shapes
void setProxyShapes(ProxyShape* convexProxyShape, ProxyShape* concaveProxyShape) {
mConvexProxyShape = convexProxyShape;
mConcaveProxyShape = concaveProxyShape;
}
/// Test collision between a triangle and the convex mesh shape
virtual void testTriangle(const Vector3* trianglePoints) override;
};
// Class SmoothMeshContactInfo
/**
* This class is used to store data about a contact with a triangle for the smooth
* mesh algorithm.
*/
class SmoothMeshContactInfo {
public:
ContactPointInfo contactInfo;
bool isFirstShapeTriangle;
Vector3 triangleVertices[3];
/// Constructor
SmoothMeshContactInfo(const ContactPointInfo& contact, bool firstShapeTriangle, const Vector3& trianglePoint1,
const Vector3& trianglePoint2, const Vector3& trianglePoint3)
: contactInfo(contact) {
isFirstShapeTriangle = firstShapeTriangle;
triangleVertices[0] = trianglePoint1;
triangleVertices[1] = trianglePoint2;
triangleVertices[2] = trianglePoint3;
}
};
struct ContactsDepthCompare {
bool operator()(const SmoothMeshContactInfo& contact1, const SmoothMeshContactInfo& contact2)
{
return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth;
}
};
/// Method used to compare two smooth mesh contact info to sort them
//inline static bool contactsDepthCompare(const SmoothMeshContactInfo& contact1,
// const SmoothMeshContactInfo& contact2) {
// return contact1.contactInfo.penetrationDepth < contact2.contactInfo.penetrationDepth;
//}
// Class SmoothCollisionNarrowPhaseCallback
/**
* This class is used as a narrow-phase callback to get narrow-phase contacts
* of the concave triangle mesh to temporary store them in order to be used in
* the smooth mesh collision algorithm if this one is enabled.
*/
class SmoothCollisionNarrowPhaseCallback : public NarrowPhaseCallback {
private:
std::vector<SmoothMeshContactInfo>& mContactPoints;
public:
// Constructor
SmoothCollisionNarrowPhaseCallback(std::vector<SmoothMeshContactInfo>& contactPoints)
: mContactPoints(contactPoints) {
}
/// Called by a narrow-phase collision algorithm when a new contact has been found
virtual void notifyContact(OverlappingPair* overlappingPair,
const ContactPointInfo& contactInfo) override;
};
// Class ConcaveVsConvexAlgorithm
/**
* This class is used to compute the narrow-phase collision detection
* between a concave collision shape and a convex collision shape. The idea is
* to use the GJK collision detection algorithm to compute the collision between
* the convex shape and each of the triangles in the concave shape.
*/
class ConcaveVsConvexAlgorithm : public NarrowPhaseAlgorithm {
protected :
// -------------------- Attributes -------------------- //
// -------------------- Methods -------------------- //
/// Process the concave triangle mesh collision using the smooth mesh collision algorithm
void processSmoothMeshCollision(OverlappingPair* overlappingPair,
std::vector<SmoothMeshContactInfo> contactPoints,
NarrowPhaseCallback* narrowPhaseCallback);
/// Add a triangle vertex into the set of processed triangles
void addProcessedVertex(std::unordered_multimap<int, Vector3>& processTriangleVertices,
const Vector3& vertex);
/// Return true if the vertex is in the set of already processed vertices
bool hasVertexBeenProcessed(const std::unordered_multimap<int, Vector3>& processTriangleVertices,
const Vector3& vertex) const;
public :
// -------------------- Methods -------------------- //
/// Constructor
ConcaveVsConvexAlgorithm() = default;
/// Destructor
virtual ~ConcaveVsConvexAlgorithm() override = default;
/// Private copy-constructor
ConcaveVsConvexAlgorithm(const ConcaveVsConvexAlgorithm& algorithm) = delete;
/// Private assignment operator
ConcaveVsConvexAlgorithm& operator=(const ConcaveVsConvexAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volume collide
virtual void testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) override;
};
// Add a triangle vertex into the set of processed triangles
inline void ConcaveVsConvexAlgorithm::addProcessedVertex(std::unordered_multimap<int, Vector3>& processTriangleVertices, const Vector3& vertex) {
processTriangleVertices.insert(std::make_pair(int(vertex.x * vertex.y * vertex.z), vertex));
}
}
#endif

58
src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.cpp Normal file
View File

@ -0,0 +1,58 @@
/********************************************************************************
* 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 "ConvexPolyhedronVsConvexPolyhedronAlgorithm.h"
#include "GJK/GJKAlgorithm.h"
#include "SAT/SATAlgorithm.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Compute the narrow-phase collision detection between two convex polyhedra
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool ConvexPolyhedronVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
MemoryAllocator& memoryAllocator) {
// Run the SAT algorithm to find the separating axis and compute contact point
SATAlgorithm satAlgorithm(memoryAllocator);
#ifdef IS_PROFILING_ACTIVE
satAlgorithm.setProfiler(mProfiler);
#endif
// Get the last frame collision info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
bool isColliding = satAlgorithm.testCollisionConvexPolyhedronVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
lastFrameCollisionInfo->wasUsingSAT = true;
lastFrameCollisionInfo->wasUsingGJK = false;
return isColliding;
}

70
src/collision/narrowphase/ConvexPolyhedronVsConvexPolyhedronAlgorithm.h Normal file
View File

@ -0,0 +1,70 @@
/********************************************************************************
* 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_CONVEX_POLYHEDRON_VS_CONVEX_POLYHEDRON_ALGORITHM_H
#define REACTPHYSICS3D_CONVEX_POLYHEDRON_VS_CONVEX_POLYHEDRON_ALGORITHM_H
// Libraries
#include "body/Body.h"
#include "constraint/ContactPoint.h"
#include "NarrowPhaseAlgorithm.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Class ConvexPolyhedronVsConvexPolyhedronAlgorithm
/**
* This class is used to compute the narrow-phase collision detection
* between two convex polyhedra.
*/
class ConvexPolyhedronVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
protected :
public :
// -------------------- Methods -------------------- //
/// Constructor
ConvexPolyhedronVsConvexPolyhedronAlgorithm() = default;
/// Destructor
virtual ~ConvexPolyhedronVsConvexPolyhedronAlgorithm() override = default;
/// Deleted copy-constructor
ConvexPolyhedronVsConvexPolyhedronAlgorithm(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Deleted assignment operator
ConvexPolyhedronVsConvexPolyhedronAlgorithm& operator=(const ConvexPolyhedronVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between two convex polyhedra
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override;
};
}
#endif

42
src/collision/narrowphase/DefaultCollisionDispatch.cpp
View File

@ -29,16 +29,6 @@
using namespace reactphysics3d;
/// Initialize the collision dispatch configuration
void DefaultCollisionDispatch::init(CollisionDetection* collisionDetection,
MemoryAllocator* memoryAllocator) {
// Initialize the collision algorithms
mSphereVsSphereAlgorithm.init(collisionDetection, memoryAllocator);
mGJKAlgorithm.init(collisionDetection, memoryAllocator);
mConcaveVsConvexAlgorithm.init(collisionDetection, memoryAllocator);
}
// Select and return the narrow-phase collision detection algorithm to
// use between two types of collision shapes.
NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int type2) {
@ -46,20 +36,34 @@ NarrowPhaseAlgorithm* DefaultCollisionDispatch::selectAlgorithm(int type1, int t
CollisionShapeType shape1Type = static_cast<CollisionShapeType>(type1);
CollisionShapeType shape2Type = static_cast<CollisionShapeType>(type2);
if (type1 > type2) {
return nullptr;
}
// Sphere vs Sphere algorithm
if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::SPHERE) {
return &mSphereVsSphereAlgorithm;
}
// Concave vs Convex algorithm
else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) ||
(!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
return &mConcaveVsConvexAlgorithm;
// Sphere vs Capsule algorithm
if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::CAPSULE) {
return &mSphereVsCapsuleAlgorithm;
}
// Convex vs Convex algorithm (GJK algorithm)
else if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) {
return &mGJKAlgorithm;
// Capsule vs Capsule algorithm
if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::CAPSULE) {
return &mCapsuleVsCapsuleAlgorithm;
}
else {
return nullptr;
// Sphere vs Convex Polyhedron algorithm
if (shape1Type == CollisionShapeType::SPHERE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) {
return &mSphereVsConvexPolyhedronAlgorithm;
}
// Capsule vs Convex Polyhedron algorithm
if (shape1Type == CollisionShapeType::CAPSULE && shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) {
return &mCapsuleVsConvexPolyhedronAlgorithm;
}
// Convex Polyhedron vs Convex Polyhedron algorithm
if (shape1Type == CollisionShapeType::CONVEX_POLYHEDRON &&
shape2Type == CollisionShapeType::CONVEX_POLYHEDRON) {
return &mConvexPolyhedronVsConvexPolyhedronAlgorithm;
}
return nullptr;
}

52
src/collision/narrowphase/DefaultCollisionDispatch.h
View File

@ -28,8 +28,12 @@
// Libraries
#include "CollisionDispatch.h"
#include "ConcaveVsConvexAlgorithm.h"
#include "SphereVsSphereAlgorithm.h"
#include "SphereVsConvexPolyhedronAlgorithm.h"
#include "SphereVsCapsuleAlgorithm.h"
#include "CapsuleVsCapsuleAlgorithm.h"
#include "CapsuleVsConvexPolyhedronAlgorithm.h"
#include "ConvexPolyhedronVsConvexPolyhedronAlgorithm.h"
#include "GJK/GJKAlgorithm.h"
namespace reactphysics3d {
@ -47,11 +51,20 @@ class DefaultCollisionDispatch : public CollisionDispatch {
/// Sphere vs Sphere collision algorithm
SphereVsSphereAlgorithm mSphereVsSphereAlgorithm;
/// Concave vs Convex collision algorithm
ConcaveVsConvexAlgorithm mConcaveVsConvexAlgorithm;
/// Capsule vs Capsule collision algorithm
CapsuleVsCapsuleAlgorithm mCapsuleVsCapsuleAlgorithm;
/// GJK Algorithm
GJKAlgorithm mGJKAlgorithm;
/// Sphere vs Capsule collision algorithm
SphereVsCapsuleAlgorithm mSphereVsCapsuleAlgorithm;
/// Sphere vs Convex Polyhedron collision algorithm
SphereVsConvexPolyhedronAlgorithm mSphereVsConvexPolyhedronAlgorithm;
/// Capsule vs Convex Polyhedron collision algorithm
CapsuleVsConvexPolyhedronAlgorithm mCapsuleVsConvexPolyhedronAlgorithm;
/// Convex Polyhedron vs Convex Polyhedron collision algorithm
ConvexPolyhedronVsConvexPolyhedronAlgorithm mConvexPolyhedronVsConvexPolyhedronAlgorithm;
public:
@ -61,15 +74,36 @@ class DefaultCollisionDispatch : public CollisionDispatch {
/// Destructor
virtual ~DefaultCollisionDispatch() override = default;
/// Initialize the collision dispatch configuration
virtual void init(CollisionDetection* collisionDetection,
MemoryAllocator* memoryAllocator) override;
/// Select and return the narrow-phase collision detection algorithm to
/// use between two types of collision shapes.
virtual NarrowPhaseAlgorithm* selectAlgorithm(int type1, int type2) override;
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
virtual void setProfiler(Profiler* profiler) override;
#endif
};
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void DefaultCollisionDispatch::setProfiler(Profiler* profiler) {
CollisionDispatch::setProfiler(profiler);
mSphereVsSphereAlgorithm.setProfiler(profiler);
mCapsuleVsCapsuleAlgorithm.setProfiler(profiler);
mSphereVsCapsuleAlgorithm.setProfiler(profiler);
mSphereVsConvexPolyhedronAlgorithm.setProfiler(profiler);
mCapsuleVsConvexPolyhedronAlgorithm.setProfiler(profiler);
mConvexPolyhedronVsConvexPolyhedronAlgorithm.setProfiler(profiler);
}
#endif
}
#endif

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

@ -1,442 +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 "EPAAlgorithm.h"
#include "engine/Profiler.h"
#include "collision/narrowphase//GJK/GJKAlgorithm.h"
#include "TrianglesStore.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Decide if the origin is in the tetrahedron.
/// Return 0 if the origin is in the tetrahedron and return the number (1,2,3 or 4) of
/// the vertex that is wrong if the origin is not in the tetrahedron
int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
const Vector3& p3, const Vector3& p4) const {
// Check vertex 1
Vector3 normal1 = (p2-p1).cross(p3-p1);
if ((normal1.dot(p1) > 0.0) == (normal1.dot(p4) > 0.0)) {
return 4;
}
// Check vertex 2
Vector3 normal2 = (p4-p2).cross(p3-p2);
if ((normal2.dot(p2) > 0.0) == (normal2.dot(p1) > 0.0)) {
return 1;
}
// Check vertex 3
Vector3 normal3 = (p4-p3).cross(p1-p3);
if ((normal3.dot(p3) > 0.0) == (normal3.dot(p2) > 0.0)) {
return 2;
}
// Check vertex 4
Vector3 normal4 = (p2-p4).cross(p1-p4);
if ((normal4.dot(p4) > 0.0) == (normal4.dot(p3) > 0.0)) {
return 3;
}
// The origin is in the tetrahedron, we return 0
return 0;
}
// Compute the penetration depth with the EPA algorithm.
/// This method computes the penetration depth and contact points between two
/// 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. 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,
const Transform& transform2,
Vector3& v,
NarrowPhaseCallback* narrowPhaseCallback) {
PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()");
decimal gjkPenDepthSquare = v.lengthSquare();
assert(shape1Info.collisionShape->isConvex());
assert(shape2Info.collisionShape->isConvex());
const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
Vector3 suppPointsA[MAX_SUPPORT_POINTS]; // Support points of object A in local coordinates
Vector3 suppPointsB[MAX_SUPPORT_POINTS]; // Support points of object B in local coordinates
Vector3 points[MAX_SUPPORT_POINTS]; // Current points
TrianglesStore triangleStore; // Store the triangles
TriangleEPA* triangleHeap[MAX_FACETS]; // Heap that contains the face
// candidate of the EPA algorithm
// Transform a point from local space of body 2 to local
// space of body 1 (the GJK algorithm is done in local space of body 1)
Transform body2Tobody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local
// space of body 1 into local space of body 2
Quaternion rotateToBody2 = transform2.getOrientation().getInverse() *
transform1.getOrientation();
// Get the simplex computed previously by the GJK algorithm
int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
// Compute the tolerance
decimal tolerance = MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint();
// Number of triangles in the polytope
unsigned int nbTriangles = 0;
// Clear the storing of triangles
triangleStore.clear();
// Select an action according to the number of points in the simplex
// computed with GJK algorithm in order to obtain an initial polytope for
// The EPA algorithm.
switch(nbVertices) {
case 1:
// 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 true;
case 2: {
// The simplex returned by GJK is a line segment d containing the origin.
// We add two additional support points to construct a hexahedron (two tetrahedron
// glued together with triangle faces. The idea is to compute three different vectors
// v1, v2 and v3 that are orthogonal to the segment d. The three vectors are relatively
// rotated of 120 degree around the d segment. The the three new points to
// construct the polytope are the three support points in those three directions
// v1, v2 and v3.
// Direction of the segment
Vector3 d = (points[1] - points[0]).getUnit();
// Choose the coordinate axis from the minimal absolute component of the vector d
int minAxis = d.getAbsoluteVector().getMinAxis();
// Compute sin(60)
const decimal sin60 = decimal(sqrt(3.0)) * decimal(0.5);
// Create a rotation quaternion to rotate the vector v1 to get the vectors
// v2 and v3
Quaternion rotationQuat(d.x * sin60, d.y * sin60, d.z * sin60, 0.5);
// Compute the vector v1, v2, v3
Vector3 v1 = d.cross(Vector3(minAxis == 0, minAxis == 1, minAxis == 2));
Vector3 v2 = rotationQuat * v1;
Vector3 v3 = rotationQuat * v2;
// Compute the support point in the direction of v1
suppPointsA[2] = shape1->getLocalSupportPointWithMargin(v1, shape1CachedCollisionData);
suppPointsB[2] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1), shape2CachedCollisionData);
points[2] = suppPointsA[2] - suppPointsB[2];
// Compute the support point in the direction of v2
suppPointsA[3] = shape1->getLocalSupportPointWithMargin(v2, shape1CachedCollisionData);
suppPointsB[3] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2), shape2CachedCollisionData);
points[3] = suppPointsA[3] - suppPointsB[3];
// Compute the support point in the direction of v3
suppPointsA[4] = shape1->getLocalSupportPointWithMargin(v3, shape1CachedCollisionData);
suppPointsB[4] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v3), shape2CachedCollisionData);
points[4] = suppPointsA[4] - suppPointsB[4];
// Now we have an hexahedron (two tetrahedron glued together). We can simply keep the
// tetrahedron that contains the origin in order that the initial polytope of the
// EPA algorithm is a tetrahedron, which is simpler to deal with.
// If the origin is in the tetrahedron of points 0, 2, 3, 4
if (isOriginInTetrahedron(points[0], points[2], points[3], points[4]) == 0) {
// We use the point 4 instead of point 1 for the initial tetrahedron
suppPointsA[1] = suppPointsA[4];
suppPointsB[1] = suppPointsB[4];
points[1] = points[4];
}
// If the origin is in the tetrahedron of points 1, 2, 3, 4
else if (isOriginInTetrahedron(points[1], points[2], points[3], points[4]) == 0) {
// We use the point 4 instead of point 0 for the initial tetrahedron
suppPointsA[0] = suppPointsA[4];
suppPointsB[0] = suppPointsB[4];
points[0] = points[4];
}
else {
// The origin is not in the initial polytope
return false;
}
// The polytope contains now 4 vertices
nbVertices = 4;
}
case 4: {
// The simplex computed by the GJK algorithm is a tetrahedron. Here we check
// if this tetrahedron contains the origin. If it is the case, we keep it and
// otherwise we remove the wrong vertex of the tetrahedron and go in the case
// where the GJK algorithm compute a simplex of three vertices.
// Check if the tetrahedron contains the origin (or wich is the wrong vertex otherwise)
int badVertex = isOriginInTetrahedron(points[0], points[1], points[2], points[3]);
// If the origin is in the tetrahedron
if (badVertex == 0) {
// The tetrahedron is a correct initial polytope for the EPA algorithm.
// Therefore, we construct the tetrahedron.
// Comstruct the 4 triangle faces of the tetrahedron
TriangleEPA* face0 = triangleStore.newTriangle(points, 0, 1, 2);
TriangleEPA* face1 = triangleStore.newTriangle(points, 0, 3, 1);
TriangleEPA* face2 = triangleStore.newTriangle(points, 0, 2, 3);
TriangleEPA* face3 = triangleStore.newTriangle(points, 1, 3, 2);
// If the constructed tetrahedron is not correct
if (!((face0 != nullptr) && (face1 != nullptr) && (face2 != nullptr) && (face3 != nullptr)
&& face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0
&& face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
return false;
}
// Associate the edges of neighbouring triangle faces
link(EdgeEPA(face0, 0), EdgeEPA(face1, 2));
link(EdgeEPA(face0, 1), EdgeEPA(face3, 2));
link(EdgeEPA(face0, 2), EdgeEPA(face2, 0));
link(EdgeEPA(face1, 0), EdgeEPA(face2, 2));
link(EdgeEPA(face1, 1), EdgeEPA(face3, 0));
link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
// Add the triangle faces in the candidate heap
addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST);
break;
}
// The tetrahedron contains a wrong vertex (the origin is not inside the tetrahedron)
// Remove the wrong vertex and continue to the next case with the
// three remaining vertices
if (badVertex < 4) {
suppPointsA[badVertex-1] = suppPointsA[3];
suppPointsB[badVertex-1] = suppPointsB[3];
points[badVertex-1] = points[3];
}
// We have removed the wrong vertex
nbVertices = 3;
}
case 3: {
// The GJK algorithm returned a triangle that contains the origin.
// We need two new vertices to create two tetrahedron. The two new
// vertices are the support points in the "n" and "-n" direction
// where "n" is the normal of the triangle. Then, we use only the
// tetrahedron that contains the origin.
// Compute the normal of the triangle
Vector3 v1 = points[1] - points[0];
Vector3 v2 = points[2] - points[0];
Vector3 n = v1.cross(v2);
// Compute the two new vertices to obtain a hexahedron
suppPointsA[3] = shape1->getLocalSupportPointWithMargin(n, shape1CachedCollisionData);
suppPointsB[3] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-n), shape2CachedCollisionData);
points[3] = suppPointsA[3] - suppPointsB[3];
suppPointsA[4] = shape1->getLocalSupportPointWithMargin(-n, shape1CachedCollisionData);
suppPointsB[4] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData);
points[4] = suppPointsA[4] - suppPointsB[4];
TriangleEPA* face0 = nullptr;
TriangleEPA* face1 = nullptr;
TriangleEPA* face2 = nullptr;
TriangleEPA* face3 = nullptr;
// If the origin is in the first tetrahedron
if (isOriginInTetrahedron(points[0], points[1],
points[2], points[3]) == 0) {
// The tetrahedron is a correct initial polytope for the EPA algorithm.
// Therefore, we construct the tetrahedron.
// Comstruct the 4 triangle faces of the tetrahedron
face0 = triangleStore.newTriangle(points, 0, 1, 2);
face1 = triangleStore.newTriangle(points, 0, 3, 1);
face2 = triangleStore.newTriangle(points, 0, 2, 3);
face3 = triangleStore.newTriangle(points, 1, 3, 2);
}
else if (isOriginInTetrahedron(points[0], points[1],
points[2], points[4]) == 0) {
// The tetrahedron is a correct initial polytope for the EPA algorithm.
// Therefore, we construct the tetrahedron.
// Comstruct the 4 triangle faces of the tetrahedron
face0 = triangleStore.newTriangle(points, 0, 1, 2);
face1 = triangleStore.newTriangle(points, 0, 4, 1);
face2 = triangleStore.newTriangle(points, 0, 2, 4);
face3 = triangleStore.newTriangle(points, 1, 4, 2);
}
else {
return false;
}
// If the constructed tetrahedron is not correct
if (!((face0 != nullptr) && (face1 != nullptr) && (face2 != nullptr) && (face3 != nullptr)
&& face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0
&& face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
return false;
}
// Associate the edges of neighbouring triangle faces
link(EdgeEPA(face0, 0), EdgeEPA(face1, 2));
link(EdgeEPA(face0, 1), EdgeEPA(face3, 2));
link(EdgeEPA(face0, 2), EdgeEPA(face2, 0));
link(EdgeEPA(face1, 0), EdgeEPA(face2, 2));
link(EdgeEPA(face1, 1), EdgeEPA(face3, 0));
link(EdgeEPA(face2, 1), EdgeEPA(face3, 1));
// Add the triangle faces in the candidate heap
addFaceCandidate(face0, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face1, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face2, triangleHeap, nbTriangles, DECIMAL_LARGEST);
addFaceCandidate(face3, triangleHeap, nbTriangles, DECIMAL_LARGEST);
nbVertices = 4;
}
break;
}
// At this point, we have a polytope that contains the origin. Therefore, we
// can run the EPA algorithm.
if (nbTriangles == 0) {
return false;
}
TriangleEPA* triangle = 0;
decimal upperBoundSquarePenDepth = DECIMAL_LARGEST;
do {
triangle = triangleHeap[0];
// Get the next candidate face (the face closest to the origin)
std::pop_heap(&triangleHeap[0], &triangleHeap[nbTriangles], mTriangleComparison);
nbTriangles--;
// 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);
break;
}
// Compute the support point of the Minkowski
// difference (A-B) in the closest point direction
suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin(
triangle->getClosestPoint(), shape1CachedCollisionData);
suppPointsB[nbVertices] = body2Tobody1 *
shape2->getLocalSupportPointWithMargin(rotateToBody2 *
(-triangle->getClosestPoint()), shape2CachedCollisionData);
points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
int indexNewVertex = nbVertices;
nbVertices++;
// Update the upper bound of the penetration depth
decimal wDotv = points[indexNewVertex].dot(triangle->getClosestPoint());
decimal wDotVSquare = wDotv * wDotv / triangle->getDistSquare();
if (wDotVSquare < upperBoundSquarePenDepth) {
upperBoundSquarePenDepth = wDotVSquare;
}
// Compute the error
decimal error = wDotv - triangle->getDistSquare();
if (error <= std::max(tolerance, REL_ERROR_SQUARE * wDotv) ||
points[indexNewVertex] == points[(*triangle)[0]] ||
points[indexNewVertex] == points[(*triangle)[1]] ||
points[indexNewVertex] == points[(*triangle)[2]]) {
break;
}
// Now, we compute the silhouette cast by the new vertex. The current triangle
// face will not be in the convex hull. We start the local recursive silhouette
// algorithm from the current triangle face.
int i = triangleStore.getNbTriangles();
if (!triangle->computeSilhouette(points, indexNewVertex, triangleStore)) {
break;
}
// Add all the new triangle faces computed with the silhouette algorithm
// to the candidates list of faces of the current polytope
while(i != triangleStore.getNbTriangles()) {
TriangleEPA* newTriangle = &triangleStore[i];
addFaceCandidate(newTriangle, triangleHeap, nbTriangles, upperBoundSquarePenDepth);
i++;
}
}
} while(nbTriangles > 0 && triangleHeap[0]->getDistSquare() <= upperBoundSquarePenDepth);
// Compute the contact info
v = transform1.getOrientation() * triangle->getClosestPoint();
Vector3 pALocal = triangle->computeClosestPointOfObject(suppPointsA);
Vector3 pBLocal = body2Tobody1.getInverse() * triangle->computeClosestPointOfObject(suppPointsB);
Vector3 normal = v.getUnit();
decimal penetrationDepth = v.length();
// If the length of the normal vector is too small, skip this contact point
if (normal.lengthSquare() < MACHINE_EPSILON) {
return false;
}
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;
}

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

@ -1,161 +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_EPA_ALGORITHM_H
#define REACTPHYSICS3D_EPA_ALGORITHM_H
// Libraries
#include "collision/narrowphase/GJK/VoronoiSimplex.h"
#include "collision/shapes/CollisionShape.h"
#include "collision/CollisionShapeInfo.h"
#include "constraint/ContactPoint.h"
#include "collision/narrowphase/NarrowPhaseAlgorithm.h"
#include "mathematics/mathematics.h"
#include "TriangleEPA.h"
#include "memory/MemoryAllocator.h"
#include <algorithm>
/// ReactPhysics3D namespace
namespace reactphysics3d {
// ---------- Constants ---------- //
/// Maximum number of support points of the polytope
constexpr unsigned int MAX_SUPPORT_POINTS = 100;
/// Maximum number of facets of the polytope
constexpr unsigned int MAX_FACETS = 200;
// Class TriangleComparison
/**
* This class allows the comparison of two triangles in the heap
* The comparison between two triangles is made using their square distance to the closest
* point to the origin. The goal is that in the heap, the first triangle is the one with the
* smallest square distance.
*/
class TriangleComparison {
public:
/// Comparison operator
bool operator()(const TriangleEPA* face1, const TriangleEPA* face2) {
return (face1->getDistSquare() > face2->getDistSquare());
}
};
// Class EPAAlgorithm
/**
* This class is the implementation of the Expanding Polytope Algorithm (EPA).
* The EPA algorithm computes the penetration depth and contact points between
* two enlarged objects (with margin) where the original objects (without margin)
* intersect. The penetration depth of a pair of intersecting objects A and B is
* the length of a point on the boundary of the Minkowski sum (A-B) closest to the
* origin. The goal of the EPA algorithm is to start with an initial simplex polytope
* that contains the origin and expend it in order to find the point on the boundary
* of (A-B) that is closest to the origin. An initial simplex that contains origin
* has been computed wit GJK algorithm. The EPA Algorithm will extend this simplex
* polytope to find the correct penetration depth. The implementation of the EPA
* algorithm is based on the book "Collision Detection in 3D Environments".
*/
class EPAAlgorithm {
private:
// -------------------- Attributes -------------------- //
/// Reference to the memory allocator
MemoryAllocator* mMemoryAllocator;
/// Triangle comparison operator
TriangleComparison mTriangleComparison;
// -------------------- Methods -------------------- //
/// Add a triangle face in the candidate triangle heap
void addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap, uint& nbTriangles,
decimal upperBoundSquarePenDepth);
/// Decide if the origin is in the tetrahedron.
int isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
const Vector3& p3, const Vector3& p4) const;
public:
// -------------------- Methods -------------------- //
/// Constructor
EPAAlgorithm() = default;
/// Destructor
~EPAAlgorithm() = default;
/// Deleted copy-constructor
EPAAlgorithm(const EPAAlgorithm& algorithm) = delete;
/// Deleted assignment operator
EPAAlgorithm& operator=(const EPAAlgorithm& algorithm) = delete;
/// Initalize the algorithm
void init(MemoryAllocator* memoryAllocator);
/// Compute the penetration depth with EPA algorithm.
bool computePenetrationDepthAndContactPoints(const VoronoiSimplex& simplex,
CollisionShapeInfo shape1Info,
const Transform& transform1,
CollisionShapeInfo shape2Info,
const Transform& transform2,
Vector3& v,
NarrowPhaseCallback* narrowPhaseCallback);
};
// Add a triangle face in the candidate triangle heap in the EPA algorithm
inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA** heap,
uint& nbTriangles, decimal upperBoundSquarePenDepth) {
// If the closest point of the affine hull of triangle
// points is internal to the triangle and if the distance
// of the closest point from the origin is at most the
// penetration depth upper bound
if (triangle->isClosestPointInternalToTriangle() &&
triangle->getDistSquare() <= upperBoundSquarePenDepth) {
// Add the triangle face to the list of candidates
heap[nbTriangles] = triangle;
nbTriangles++;
std::push_heap(&heap[0], &heap[nbTriangles], mTriangleComparison);
}
}
// Initalize the algorithm
inline void EPAAlgorithm::init(MemoryAllocator* memoryAllocator) {
mMemoryAllocator = memoryAllocator;
}
}
#endif

125
src/collision/narrowphase/EPA/EdgeEPA.cpp
View File

@ -1,125 +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 "EdgeEPA.h"
#include "TriangleEPA.h"
#include "TrianglesStore.h"
#include <cassert>
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
EdgeEPA::EdgeEPA(TriangleEPA* ownerTriangle, int index)
: mOwnerTriangle(ownerTriangle), mIndex(index) {
assert(index >= 0 && index < 3);
}
// Copy-constructor
EdgeEPA::EdgeEPA(const EdgeEPA& edge) {
mOwnerTriangle = edge.mOwnerTriangle;
mIndex = edge.mIndex;
}
// Return the index of the source vertex of the edge (vertex starting the edge)
uint EdgeEPA::getSourceVertexIndex() const {
return (*mOwnerTriangle)[mIndex];
}
// Return the index of the target vertex of the edge (vertex ending the edge)
uint EdgeEPA::getTargetVertexIndex() const {
return (*mOwnerTriangle)[indexOfNextCounterClockwiseEdge(mIndex)];
}
// Execute the recursive silhouette algorithm from this edge
bool EdgeEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex,
TrianglesStore& triangleStore) {
// If the edge has not already been visited
if (!mOwnerTriangle->getIsObsolete()) {
// If the triangle of this edge is not visible from the given point
if (!mOwnerTriangle->isVisibleFromVertex(vertices, indexNewVertex)) {
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
getTargetVertexIndex(),
getSourceVertexIndex());
// If the triangle has been created
if (triangle != nullptr) {
halfLink(EdgeEPA(triangle, 1), *this);
return true;
}
return false;
}
else {
// The current triangle is visible and therefore obsolete
mOwnerTriangle->setIsObsolete(true);
int backup = triangleStore.getNbTriangles();
if(!mOwnerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(
this->mIndex)).computeSilhouette(vertices,
indexNewVertex,
triangleStore)) {
mOwnerTriangle->setIsObsolete(false);
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
getTargetVertexIndex(),
getSourceVertexIndex());
// If the triangle has been created
if (triangle != nullptr) {
halfLink(EdgeEPA(triangle, 1), *this);
return true;
}
return false;
}
else if (!mOwnerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(
this->mIndex)).computeSilhouette(vertices,
indexNewVertex,
triangleStore)) {
mOwnerTriangle->setIsObsolete(false);
triangleStore.setNbTriangles(backup);
TriangleEPA* triangle = triangleStore.newTriangle(vertices, indexNewVertex,
getTargetVertexIndex(),
getSourceVertexIndex());
if (triangle != nullptr) {
halfLink(EdgeEPA(triangle, 1), *this);
return true;
}
return false;
}
}
}
return true;
}

145
src/collision/narrowphase/EPA/TriangleEPA.cpp
View File

@ -1,145 +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 "TriangleEPA.h"
#include "EdgeEPA.h"
#include "TrianglesStore.h"
// We use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
TriangleEPA::TriangleEPA(uint indexVertex1, uint indexVertex2, uint indexVertex3)
: mIsObsolete(false) {
mIndicesVertices[0] = indexVertex1;
mIndicesVertices[1] = indexVertex2;
mIndicesVertices[2] = indexVertex3;
}
// Compute the point v closest to the origin of this triangle
bool TriangleEPA::computeClosestPoint(const Vector3* vertices) {
const Vector3& p0 = vertices[mIndicesVertices[0]];
Vector3 v1 = vertices[mIndicesVertices[1]] - p0;
Vector3 v2 = vertices[mIndicesVertices[2]] - p0;
decimal v1Dotv1 = v1.dot(v1);
decimal v1Dotv2 = v1.dot(v2);
decimal v2Dotv2 = v2.dot(v2);
decimal p0Dotv1 = p0.dot(v1);
decimal p0Dotv2 = p0.dot(v2);
// Compute determinant
mDet = v1Dotv1 * v2Dotv2 - v1Dotv2 * v1Dotv2;
// Compute lambda values
mLambda1 = p0Dotv2 * v1Dotv2 - p0Dotv1 * v2Dotv2;
mLambda2 = p0Dotv1 * v1Dotv2 - p0Dotv2 * v1Dotv1;
// If the determinant is positive
if (mDet > 0.0) {
// Compute the closest point v
mClosestPoint = p0 + decimal(1.0) / mDet * (mLambda1 * v1 + mLambda2 * v2);
// Compute the square distance of closest point to the origin
mDistSquare = mClosestPoint.dot(mClosestPoint);
return true;
}
return false;
}
/// Link an edge with another one. It means that the current edge of a triangle will
/// be associated with the edge of another triangle in order that both triangles
/// are neighbour along both edges).
bool reactphysics3d::link(const EdgeEPA& edge0, const EdgeEPA& edge1) {
bool isPossible = (edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() &&
edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex());
if (isPossible) {
edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1;
edge1.getOwnerTriangle()->mAdjacentEdges[edge1.getIndex()] = edge0;
}
return isPossible;
}
/// Make an half link of an edge with another one from another triangle. An half-link
/// between an edge "edge0" and an edge "edge1" represents the fact that "edge1" is an
/// adjacent edge of "edge0" but not the opposite. The opposite edge connection will
/// be made later.
void reactphysics3d::halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1) {
assert(edge0.getSourceVertexIndex() == edge1.getTargetVertexIndex() &&
edge0.getTargetVertexIndex() == edge1.getSourceVertexIndex());
// Link
edge0.getOwnerTriangle()->mAdjacentEdges[edge0.getIndex()] = edge1;
}
// Execute the recursive silhouette algorithm from this triangle face.
/// The parameter "vertices" is an array that contains the vertices of the current polytope and the
/// parameter "indexNewVertex" is the index of the new vertex in this array. The goal of the
/// silhouette algorithm is to add the new vertex in the polytope by keeping it convex. Therefore,
/// the triangle faces that are visible from the new vertex must be removed from the polytope and we
/// need to add triangle faces where each face contains the new vertex and an edge of the silhouette.
/// The silhouette is the connected set of edges that are part of the border between faces that
/// are seen and faces that are not seen from the new vertex. This method starts from the nearest
/// face from the new vertex, computes the silhouette and create the new faces from the new vertex in
/// order that we always have a convex polytope. The faces visible from the new vertex are set
/// obselete and will not be considered as being a candidate face in the future.
bool TriangleEPA::computeSilhouette(const Vector3* vertices, uint indexNewVertex,
TrianglesStore& triangleStore) {
uint first = triangleStore.getNbTriangles();
// Mark the current triangle as obsolete because it
setIsObsolete(true);
// Execute recursively the silhouette algorithm for the adjacent edges of neighboring
// triangles of the current triangle
bool result = mAdjacentEdges[0].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
mAdjacentEdges[1].computeSilhouette(vertices, indexNewVertex, triangleStore) &&
mAdjacentEdges[2].computeSilhouette(vertices, indexNewVertex, triangleStore);
if (result) {
int i,j;
// For each triangle face that contains the new vertex and an edge of the silhouette
for (i=first, j=triangleStore.getNbTriangles()-1;
i != triangleStore.getNbTriangles(); j = i++) {
TriangleEPA* triangle = &triangleStore[i];
halfLink(triangle->getAdjacentEdge(1), EdgeEPA(triangle, 1));
if (!link(EdgeEPA(triangle, 0), EdgeEPA(&triangleStore[j], 2))) {
return false;
}
}
}
return result;
}

197
src/collision/narrowphase/EPA/TriangleEPA.h
View File

@ -1,197 +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_TRIANGLE_EPA_H
#define REACTPHYSICS3D_TRIANGLE_EPA_H
// Libraries
#include "mathematics/mathematics.h"
#include "configuration.h"
#include "EdgeEPA.h"
#include <cassert>
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Prototypes
bool link(const EdgeEPA& edge0, const EdgeEPA& edge1);
void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1);
// Class TriangleEPA
/**
* This class represents a triangle face of the current polytope in the EPA algorithm.
*/
class TriangleEPA {
private:
// -------------------- Attributes -------------------- //
/// Indices of the vertices y_i of the triangle
uint mIndicesVertices[3];
/// Three adjacent edges of the triangle (edges of other triangles)
EdgeEPA mAdjacentEdges[3];
/// True if the triangle face is visible from the new support point
bool mIsObsolete;
/// Determinant
decimal mDet;
/// Point v closest to the origin on the affine hull of the triangle
Vector3 mClosestPoint;
/// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
decimal mLambda1;
/// Lambda1 value such that v = lambda0 * y_0 + lambda1 * y_1 + lambda2 * y_2
decimal mLambda2;
/// Square distance of the point closest point v to the origin
decimal mDistSquare;
public:
// -------------------- Methods -------------------- //
/// Constructor
TriangleEPA() = default;
/// Constructor
TriangleEPA(uint v1, uint v2, uint v3);
/// Destructor
~TriangleEPA() = default;
/// Deleted copy-constructor
TriangleEPA(const TriangleEPA& triangle) = delete;
/// Deleted assignment operator
TriangleEPA& operator=(const TriangleEPA& triangle) = delete;
/// Return an adjacent edge of the triangle
EdgeEPA& getAdjacentEdge(int index);
/// Set an adjacent edge of the triangle
void setAdjacentEdge(int index, EdgeEPA& edge);
/// Return the square distance of the closest point to origin
decimal getDistSquare() const;
/// Set the isObsolete value
void setIsObsolete(bool isObsolete);
/// Return true if the triangle face is obsolete
bool getIsObsolete() const;
/// Return the point closest to the origin
const Vector3& getClosestPoint() const;
// Return true if the closest point on affine hull is inside the triangle
bool isClosestPointInternalToTriangle() const;
/// Return true if the triangle is visible from a given vertex
bool isVisibleFromVertex(const Vector3* vertices, uint index) const;
/// Compute the point v closest to the origin of this triangle
bool computeClosestPoint(const Vector3* vertices);
/// Compute the point of an object closest to the origin
Vector3 computeClosestPointOfObject(const Vector3* supportPointsOfObject) const;
/// Execute the recursive silhouette algorithm from this triangle face.
bool computeSilhouette(const Vector3* vertices, uint index, TrianglesStore& triangleStore);
/// Access operator
uint operator[](int i) const;
/// Associate two edges
friend bool link(const EdgeEPA& edge0, const EdgeEPA& edge1);
/// Make a half-link between two edges
friend void halfLink(const EdgeEPA& edge0, const EdgeEPA& edge1);
};
// Return an edge of the triangle
inline EdgeEPA& TriangleEPA::getAdjacentEdge(int index) {
assert(index >= 0 && index < 3);
return mAdjacentEdges[index];
}
// Set an adjacent edge of the triangle
inline void TriangleEPA::setAdjacentEdge(int index, EdgeEPA& edge) {
assert(index >=0 && index < 3);
mAdjacentEdges[index] = edge;
}
// Return the square distance of the closest point to origin
inline decimal TriangleEPA::getDistSquare() const {
return mDistSquare;
}
// Set the isObsolete value
inline void TriangleEPA::setIsObsolete(bool isObsolete) {
mIsObsolete = isObsolete;
}
// Return true if the triangle face is obsolete
inline bool TriangleEPA::getIsObsolete() const {
return mIsObsolete;
}
// Return the point closest to the origin
inline const Vector3& TriangleEPA::getClosestPoint() const {
return mClosestPoint;
}
// Return true if the closest point on affine hull is inside the triangle
inline bool TriangleEPA::isClosestPointInternalToTriangle() const {
return (mLambda1 >= 0.0 && mLambda2 >= 0.0 && (mLambda1 + mLambda2) <= mDet);
}
// Return true if the triangle is visible from a given vertex
inline bool TriangleEPA::isVisibleFromVertex(const Vector3* vertices, uint index) const {
Vector3 closestToVert = vertices[index] - mClosestPoint;
return (mClosestPoint.dot(closestToVert) > 0.0);
}
// Compute the point of an object closest to the origin
inline Vector3 TriangleEPA::computeClosestPointOfObject(const Vector3* supportPointsOfObject) const{
const Vector3& p0 = supportPointsOfObject[mIndicesVertices[0]];
return p0 + decimal(1.0)/mDet * (mLambda1 * (supportPointsOfObject[mIndicesVertices[1]] - p0) +
mLambda2 * (supportPointsOfObject[mIndicesVertices[2]] - p0));
}
// Access operator
inline uint TriangleEPA::operator[](int i) const {
assert(i >= 0 && i <3);
return mIndicesVertices[i];
}
}
#endif

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

@ -26,6 +26,8 @@
// Libraries
#include "GJKAlgorithm.h"
#include "constraint/ContactPoint.h"
#include "engine/OverlappingPair.h"
#include "collision/shapes/TriangleShape.h"
#include "configuration.h"
#include "engine/Profiler.h"
#include <algorithm>
@ -36,11 +38,6 @@
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
GJKAlgorithm::GJKAlgorithm() : NarrowPhaseAlgorithm() {
}
// Compute a contact info if the two collision shapes collide.
/// This method implements the Hybrid Technique for computing the penetration depth by
/// running the GJK algorithm on original objects (without margin). If the shapes intersect
@ -50,11 +47,9 @@ GJKAlgorithm::GJKAlgorithm() : NarrowPhaseAlgorithm() {
/// algorithm on the enlarged object to obtain a simplex polytope that contains the
/// origin, they we give that simplex polytope to the EPA algorithm which will compute
/// the correct penetration depth and contact points between the enlarged objects.
void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) {
GJKAlgorithm::GJKResult GJKAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) {
PROFILE("GJKAlgorithm::testCollision()");
PROFILE("GJKAlgorithm::testCollision()", mProfiler);
Vector3 suppA; // Support point of object A
Vector3 suppB; // Support point of object B
@ -65,40 +60,45 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
decimal prevDistSquare;
bool contactFound = false;
assert(shape1Info.collisionShape->isConvex());
assert(shape2Info.collisionShape->isConvex());
assert(narrowPhaseInfo->collisionShape1->isConvex());
assert(narrowPhaseInfo->collisionShape2->isConvex());
const ConvexShape* shape1 = static_cast<const ConvexShape*>(shape1Info.collisionShape);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(shape2Info.collisionShape);
void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
bool isPolytopeShape = shape1->isPolyhedron() && shape2->isPolyhedron();
const ConvexShape* shape1 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape1);
const ConvexShape* shape2 = static_cast<const ConvexShape*>(narrowPhaseInfo->collisionShape2);
// Get the local-space to world-space transforms
const Transform transform1 = shape1Info.shapeToWorldTransform;
const Transform transform2 = shape2Info.shapeToWorldTransform;
const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
// Transform a point from local space of body 2 to local
// space of body 1 (the GJK algorithm is done in local space of body 1)
Transform body2Tobody1 = transform1.getInverse() * transform2;
Transform transform1Inverse = transform1.getInverse();
Transform body2Tobody1 = transform1Inverse * transform2;
// Matrix that transform a direction from local
// Quaternion that transform a direction from local
// space of body 1 into local space of body 2
Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
transform1.getOrientation().getMatrix();
Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * transform1.getOrientation();
// Initialize the margin (sum of margins of both objects)
decimal margin = shape1->getMargin() + shape2->getMargin();
decimal marginSquare = margin * margin;
assert(margin > 0.0);
assert(margin > decimal(0.0));
// Create a simplex set
VoronoiSimplex simplex;
// Get the last collision frame info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
// Get the previous point V (last cached separating axis)
Vector3 v = mCurrentOverlappingPair->getCachedSeparatingAxis();
Vector3 v;
if (lastFrameCollisionInfo->isValid && lastFrameCollisionInfo->wasUsingGJK) {
v = lastFrameCollisionInfo->gjkSeparatingAxis;
assert(v.lengthSquare() > decimal(0.000001));
}
else {
v.setAllValues(0, 1, 0);
}
// Initialize the upper bound for the square distance
decimal distSquare = DECIMAL_LARGEST;
@ -106,9 +106,8 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
do {
// Compute the support points for original objects (without margins) A and B
suppA = shape1->getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData);
suppB = body2Tobody1 *
shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData);
suppA = shape1->getLocalSupportPointWithoutMargin(-v);
suppB = body2Tobody1 * shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
@ -116,13 +115,13 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
vDotw = v.dot(w);
// If the enlarge objects (with margins) do not intersect
if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) {
if (vDotw > decimal(0.0) && vDotw * vDotw > distSquare * marginSquare) {
// Cache the current separating axis for frame coherence
mCurrentOverlappingPair->setCachedSeparatingAxis(v);
lastFrameCollisionInfo->gjkSeparatingAxis = v;
// No intersection, we return
return;
return GJKResult::SEPARATED;
}
// If the objects intersect only in the margins
@ -153,12 +152,6 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
break;
}
// 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
prevDistSquare = distSquare;
distSquare = v.lengthSquare();
@ -176,23 +169,9 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
break;
}
} while((isPolytopeShape && !simplex.isFull()) || (!isPolytopeShape && !simplex.isFull() &&
distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint()));
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint());
bool isEPAResultValid = false;
// If no contact has been found (penetration case)
if (!contactFound) {
// 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) {
if (contactFound && distSquare > MACHINE_EPSILON) {
// Compute the closet points of both objects (without the margins)
simplex.computeClosestPointsOfAandB(pA, pB);
@ -200,7 +179,7 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
// 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);
assert(dist > decimal(0.0));
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
@ -209,109 +188,31 @@ void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
decimal penetrationDepth = margin - dist;
// If the penetration depth is negative (due too numerical errors), there is no contact
if (penetrationDepth <= 0.0) {
return;
if (penetrationDepth <= decimal(0.0)) {
return GJKResult::SEPARATED;
}
// Do not generate a contact point with zero normal length
if (normal.lengthSquare() < MACHINE_EPSILON) {
return;
return GJKResult::SEPARATED;
}
// Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
if (reportContacts) {
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// Compute smooth triangle mesh contact if one of the two collision shapes is a triangle
TriangleShape::computeSmoothTriangleMeshContact(shape1, shape2, pA, pB, transform1, transform2,
penetrationDepth, normal);
// Add a new contact point
narrowPhaseInfo->addContactPoint(normal, penetrationDepth, pA, pB);
}
return GJKResult::COLLIDE_IN_MARGIN;
}
return GJKResult::INTERPENETRATE;
}
/// This method runs the GJK algorithm on the two enlarged objects (with margin)
/// to compute a simplex polytope that contains the origin. The two objects are
/// 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.
bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
const Transform& transform1,
const CollisionShapeInfo& shape2Info,
const Transform& transform2,
NarrowPhaseCallback* narrowPhaseCallback,
Vector3& v) {
PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()");
VoronoiSimplex simplex;
Vector3 suppA;
Vector3 suppB;
Vector3 w;
decimal vDotw;
decimal distSquare = DECIMAL_LARGEST;
decimal prevDistSquare;
assert(shape1Info.collisionShape->isConvex());
assert(shape2Info.collisionShape->isConvex());
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;
// Transform a point from local space of body 2 to local space
// of body 1 (the GJK algorithm is done in local space of body 1)
Transform body2ToBody1 = transform1.getInverse() * transform2;
// Matrix that transform a direction from local space of body 1 into local space of body 2
Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() *
transform1.getOrientation().getMatrix();
do {
// Compute the support points for the enlarged object A and B
suppA = shape1->getLocalSupportPointWithMargin(-v, shape1CachedCollisionData);
suppB = body2ToBody1 * shape2->getLocalSupportPointWithMargin(rotateToBody2 * v, shape2CachedCollisionData);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
vDotw = v.dot(w);
// If the enlarge objects do not intersect
if (vDotw > 0.0) {
// No intersection, we return
return false;
}
// Add the new support point to the simplex
simplex.addPoint(w, suppA, suppB);
if (simplex.isAffinelyDependent()) {
return false;
}
if (!simplex.computeClosestPoint(v)) {
return false;
}
// Store and update the square distance
prevDistSquare = distSquare;
distSquare = v.lengthSquare();
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
return false;
}
} 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
// between the two enlarged objects
return mAlgoEPA.computePenetrationDepthAndContactPoints(simplex, shape1Info,
transform1, shape2Info, transform2,
v, narrowPhaseCallback);
}
// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) {
@ -324,8 +225,6 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
const ConvexShape* shape = static_cast<const ConvexShape*>(proxyShape->getCollisionShape());
void** shapeCachedCollisionData = proxyShape->getCachedCollisionData();
// Support point of object B (object B is a single point)
const Vector3 suppB(localPoint);
@ -341,7 +240,7 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
do {
// Compute the support points for original objects (without margins) A and B
suppA = shape->getLocalSupportPointWithoutMargin(-v, shapeCachedCollisionData);
suppA = shape->getLocalSupportPointWithoutMargin(-v);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
@ -378,7 +277,6 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
return true;
}
// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
/// This method implements the GJK ray casting algorithm described by Gino Van Den Bergen in
/// "Ray Casting against General Convex Objects with Application to Continuous Collision Detection".
@ -388,8 +286,6 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
const ConvexShape* shape = static_cast<const ConvexShape*>(proxyShape->getCollisionShape());
void** shapeCachedCollisionData = proxyShape->getCachedCollisionData();
Vector3 suppA; // Current lower bound point on the ray (starting at ray's origin)
Vector3 suppB; // Support point on the collision shape
const decimal machineEpsilonSquare = MACHINE_EPSILON * MACHINE_EPSILON;
@ -409,7 +305,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0));
decimal lambda = decimal(0.0);
suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin)
suppB = shape->getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData);
suppB = shape->getLocalSupportPointWithoutMargin(rayDirection);
Vector3 v = suppA - suppB;
decimal vDotW, vDotR;
decimal distSquare = v.lengthSquare();
@ -419,7 +315,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo&
while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) {
// Compute the support points
suppB = shape->getLocalSupportPointWithoutMargin(v, shapeCachedCollisionData);
suppB = shape->getLocalSupportPointWithoutMargin(v);
w = suppA - suppB;
vDotW = v.dot(w);

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

@ -27,11 +27,10 @@
#define REACTPHYSICS3D_GJK_ALGORITHM_H
// Libraries
#include "collision/narrowphase/NarrowPhaseAlgorithm.h"
#include "constraint/ContactPoint.h"
#include "collision/ContactManifoldInfo.h"
#include "collision/NarrowPhaseInfo.h"
#include "collision/shapes/ConvexShape.h"
#include "collision/narrowphase/EPA/EPAAlgorithm.h"
#include "VoronoiSimplex.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@ -57,31 +56,33 @@ constexpr int MAX_ITERATIONS_GJK_RAYCAST = 32;
* Polytope Algorithm) to compute the correct penetration depth between the
* enlarged objects.
*/
class GJKAlgorithm : public NarrowPhaseAlgorithm {
class GJKAlgorithm {
private :
// -------------------- Attributes -------------------- //
/// EPA Algorithm
EPAAlgorithm mAlgoEPA;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
// -------------------- Methods -------------------- //
/// Compute the penetration depth for enlarged objects.
bool computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
const Transform& transform1,
const CollisionShapeInfo& shape2Info,
const Transform& transform2,
NarrowPhaseCallback* narrowPhaseCallback,
Vector3& v);
public :
enum class GJKResult {
SEPARATED, // The two shapes are separated outside the margin
COLLIDE_IN_MARGIN, // The two shapes overlap only in the margin (shallow penetration)
INTERPENETRATE // The two shapes overlap event without the margin (deep penetration)
};
// -------------------- Methods -------------------- //
/// Constructor
GJKAlgorithm();
GJKAlgorithm() = default;
/// Destructor
~GJKAlgorithm() = default;
@ -92,29 +93,33 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
/// Deleted assignment operator
GJKAlgorithm& operator=(const GJKAlgorithm& algorithm) = delete;
/// Initalize the algorithm
virtual void init(CollisionDetection* collisionDetection,
MemoryAllocator* memoryAllocator) override;
/// Compute a contact info if the two bounding volumes collide.
virtual void testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) override;
GJKResult testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts);
/// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape);
/// Ray casting algorithm agains a convex collision shape using the GJK Algorithm
bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo);
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
void setProfiler(Profiler* profiler);
#endif
};
// Initalize the algorithm
inline void GJKAlgorithm::init(CollisionDetection* collisionDetection,
MemoryAllocator* memoryAllocator) {
NarrowPhaseAlgorithm::init(collisionDetection, memoryAllocator);
mAlgoEPA.init(memoryAllocator);
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void GJKAlgorithm::setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
#endif
}
#endif

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

@ -563,7 +563,6 @@ bool VoronoiSimplex::computeClosestPointOnTetrahedron(const Vector3& a, const Ve
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);

49
src/collision/narrowphase/NarrowPhaseAlgorithm.h
View File

@ -28,10 +28,10 @@
// Libraries
#include "body/Body.h"
#include "constraint/ContactPoint.h"
#include "memory/MemoryAllocator.h"
#include "collision/ContactManifoldInfo.h"
#include "memory/PoolAllocator.h"
#include "engine/OverlappingPair.h"
#include "collision/CollisionShapeInfo.h"
#include "collision/NarrowPhaseInfo.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
@ -67,21 +67,19 @@ class NarrowPhaseAlgorithm {
// -------------------- Attributes -------------------- //
/// Pointer to the collision detection object
CollisionDetection* mCollisionDetection;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the memory allocator
MemoryAllocator* mMemoryAllocator;
/// Pointer to the profiler
Profiler* mProfiler;
/// Overlapping pair of the bodies currently tested for collision
OverlappingPair* mCurrentOverlappingPair;
#endif
public :
// -------------------- Methods -------------------- //
/// Constructor
NarrowPhaseAlgorithm();
NarrowPhaseAlgorithm() = default;
/// Destructor
virtual ~NarrowPhaseAlgorithm() = default;
@ -92,22 +90,27 @@ class NarrowPhaseAlgorithm {
/// Deleted assignment operator
NarrowPhaseAlgorithm& operator=(const NarrowPhaseAlgorithm& algorithm) = delete;
/// Initalize the algorithm
virtual void init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator);
/// Set the current overlapping pair of bodies
void setCurrentOverlappingPair(OverlappingPair* overlappingPair);
/// Compute a contact info if the two bounding volumes collide
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
MemoryAllocator& memoryAllocator)=0;
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
void setProfiler(Profiler* profiler);
#endif
/// Compute a contact info if the two bounding volume collide
virtual void testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback)=0;
};
// Set the current overlapping pair of bodies
inline void NarrowPhaseAlgorithm::setCurrentOverlappingPair(OverlappingPair* overlappingPair) {
mCurrentOverlappingPair = overlappingPair;
}
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void NarrowPhaseAlgorithm::setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
#endif
}

1050
src/collision/narrowphase/SAT/SATAlgorithm.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

170
src/collision/narrowphase/SAT/SATAlgorithm.h Normal file
View File

@ -0,0 +1,170 @@
/********************************************************************************
* 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_SAT_ALGORITHM_H
#define REACTPHYSICS3D_SAT_ALGORITHM_H
// Libraries
#include "collision/ContactManifoldInfo.h"
#include "collision/NarrowPhaseInfo.h"
#include "collision/shapes/ConvexPolyhedronShape.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
class CapsuleShape;
class SphereShape;
// Class SATAlgorithm
class SATAlgorithm {
private :
// -------------------- Attributes -------------------- //
/// Bias used to make sure the SAT algorithm returns the same penetration axis between frames
/// when there are multiple separating axis with the same penetration depth. The goal is to
/// make sure the contact manifold does not change too much between frames.
static const decimal SAME_SEPARATING_AXIS_BIAS;
/// Memory allocator
MemoryAllocator& mMemoryAllocator;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
// -------------------- Methods -------------------- //
/// Return true if two edges of two polyhedrons build a minkowski face (and can therefore be a separating axis)
bool testEdgesBuildMinkowskiFace(const ConvexPolyhedronShape* polyhedron1, const HalfEdgeStructure::Edge& edge1,
const ConvexPolyhedronShape* polyhedron2, const HalfEdgeStructure::Edge& edge2,
const Transform& polyhedron1ToPolyhedron2) const;
/// Return true if the arcs AB and CD on the Gauss Map intersect
bool testGaussMapArcsIntersect(const Vector3& a, const Vector3& b,
const Vector3& c, const Vector3& d,
const Vector3& bCrossA, const Vector3& dCrossC) const;
/// Compute and return the distance between the two edges in the direction of the candidate separating axis
decimal computeDistanceBetweenEdges(const Vector3& edge1A, const Vector3& edge2A, const Vector3& polyhedron2Centroid,
const Vector3& edge1Direction, const Vector3& edge2Direction,
Vector3& outSeparatingAxis) const;
/// Return the penetration depth between two polyhedra along a face normal axis of the first polyhedron
decimal testSingleFaceDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1,
const ConvexPolyhedronShape* polyhedron2,
const Transform& polyhedron1ToPolyhedron2,
uint faceIndex) const;
/// Test all the normals of a polyhedron for separating axis in the polyhedron vs polyhedron case
decimal testFacesDirectionPolyhedronVsPolyhedron(const ConvexPolyhedronShape* polyhedron1, const ConvexPolyhedronShape* polyhedron2,
const Transform& polyhedron1ToPolyhedron2, uint& minFaceIndex) const;
/// Compute the penetration depth between a face of the polyhedron and a sphere along the polyhedron face normal direction
decimal computePolyhedronFaceVsSpherePenetrationDepth(uint faceIndex, const ConvexPolyhedronShape* polyhedron,
const SphereShape* sphere, const Vector3& sphereCenter) const;
/// Compute the penetration depth between the face of a polyhedron and a capsule along the polyhedron face normal direction
decimal computePolyhedronFaceVsCapsulePenetrationDepth(uint polyhedronFaceIndex, const ConvexPolyhedronShape* polyhedron,
const CapsuleShape* capsule, const Transform& polyhedronToCapsuleTransform,
Vector3& outFaceNormalCapsuleSpace) const;
/// Compute the penetration depth when the separating axis is the cross product of polyhedron edge and capsule inner segment
decimal computeEdgeVsCapsuleInnerSegmentPenetrationDepth(const ConvexPolyhedronShape* polyhedron, const CapsuleShape* capsule,
const Vector3& capsuleSegmentAxis, const Vector3& edgeVertex1,
const Vector3& edgeDirectionCapsuleSpace,
const Transform& polyhedronToCapsuleTransform, Vector3& outAxis) const;
/// Compute the contact points between two faces of two convex polyhedra.
bool computePolyhedronVsPolyhedronFaceContactPoints(bool isMinPenetrationFaceNormalPolyhedron1, const ConvexPolyhedronShape* polyhedron1,
const ConvexPolyhedronShape* polyhedron2, const Transform& polyhedron1ToPolyhedron2,
const Transform& polyhedron2ToPolyhedron1, uint minFaceIndex,
NarrowPhaseInfo* narrowPhaseInfo, decimal minPenetrationDepth) const;
public :
// -------------------- Methods -------------------- //
/// Constructor
SATAlgorithm(MemoryAllocator& memoryAllocator);
/// Destructor
~SATAlgorithm() = default;
/// Deleted copy-constructor
SATAlgorithm(const SATAlgorithm& algorithm) = delete;
/// Deleted assignment operator
SATAlgorithm& operator=(const SATAlgorithm& algorithm) = delete;
/// Test collision between a sphere and a convex mesh
bool testCollisionSphereVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const;
/// Test collision between a capsule and a convex mesh
bool testCollisionCapsuleVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const;
/// Compute the two contact points between a polyhedron and a capsule when the separating axis is a face normal of the polyhedron
bool computeCapsulePolyhedronFaceContactPoints(uint referenceFaceIndex, decimal capsuleRadius, const ConvexPolyhedronShape* polyhedron,
decimal penetrationDepth, const Transform& polyhedronToCapsuleTransform,
Vector3& normalWorld, const Vector3& separatingAxisCapsuleSpace,
const Vector3& capsuleSegAPolyhedronSpace, const Vector3& capsuleSegBPolyhedronSpace,
NarrowPhaseInfo* narrowPhaseInfo, bool isCapsuleShape1) const;
// This method returns true if an edge of a polyhedron and a capsule forms a face of the Minkowski Difference
bool isMinkowskiFaceCapsuleVsEdge(const Vector3& capsuleSegment, const Vector3& edgeAdjacentFace1Normal,
const Vector3& edgeAdjacentFace2Normal) const;
/// Test collision between two convex meshes
bool testCollisionConvexPolyhedronVsConvexPolyhedron(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts) const;
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
void setProfiler(Profiler* profiler);
#endif
};
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void SATAlgorithm::setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
#endif
}
#endif

134
src/collision/narrowphase/SphereVsCapsuleAlgorithm.cpp Executable file
View File

@ -0,0 +1,134 @@
/********************************************************************************
* 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 "SphereVsCapsuleAlgorithm.h"
#include "collision/shapes/SphereShape.h"
#include "collision/shapes/CapsuleShape.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Compute the narrow-phase collision detection between a sphere and a capsule
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool SphereVsCapsuleAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
MemoryAllocator& memoryAllocator) {
bool isSphereShape1 = narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE;
assert(isSphereShape1 || narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CAPSULE);
// Get the collision shapes
const SphereShape* sphereShape = static_cast<const SphereShape*>(isSphereShape1 ? narrowPhaseInfo->collisionShape1 : narrowPhaseInfo->collisionShape2);
const CapsuleShape* capsuleShape = static_cast<const CapsuleShape*>(isSphereShape1 ? narrowPhaseInfo->collisionShape2 : narrowPhaseInfo->collisionShape1);
// Get the transform from sphere local-space to capsule local-space
const Transform& sphereToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape1ToWorldTransform : narrowPhaseInfo->shape2ToWorldTransform;
const Transform& capsuleToWorldTransform = isSphereShape1 ? narrowPhaseInfo->shape2ToWorldTransform : narrowPhaseInfo->shape1ToWorldTransform;
const Transform worldToCapsuleTransform = capsuleToWorldTransform.getInverse();
const Transform sphereToCapsuleSpaceTransform = worldToCapsuleTransform * sphereToWorldTransform;
// Transform the center of the sphere into the local-space of the capsule shape
const Vector3 sphereCenter = sphereToCapsuleSpaceTransform.getPosition();
// Compute the end-points of the inner segment of the capsule
const decimal capsuleHalfHeight = capsuleShape->getHeight() * decimal(0.5);
const Vector3 capsuleSegA(0, -capsuleHalfHeight, 0);
const Vector3 capsuleSegB(0, capsuleHalfHeight, 0);
// Compute the point on the inner capsule segment that is the closes to center of sphere
const Vector3 closestPointOnSegment = computeClosestPointOnSegment(capsuleSegA, capsuleSegB, sphereCenter);
// Compute the distance between the sphere center and the closest point on the segment
Vector3 sphereCenterToSegment = (closestPointOnSegment - sphereCenter);
const decimal sphereSegmentDistanceSquare = sphereCenterToSegment.lengthSquare();
// Compute the sum of the radius of the sphere and the capsule (virtual sphere)
decimal sumRadius = sphereShape->getRadius() + capsuleShape->getRadius();
// If the collision shapes overlap
if (sphereSegmentDistanceSquare < sumRadius * sumRadius) {
decimal penetrationDepth;
Vector3 normalWorld;
Vector3 contactPointSphereLocal;
Vector3 contactPointCapsuleLocal;
if (reportContacts) {
// If the sphere center is not on the capsule inner segment
if (sphereSegmentDistanceSquare > MACHINE_EPSILON) {
decimal sphereSegmentDistance = std::sqrt(sphereSegmentDistanceSquare);
sphereCenterToSegment /= sphereSegmentDistance;
contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + sphereCenterToSegment * sphereShape->getRadius());
contactPointCapsuleLocal = closestPointOnSegment - sphereCenterToSegment * capsuleShape->getRadius();
normalWorld = capsuleToWorldTransform.getOrientation() * sphereCenterToSegment;
penetrationDepth = sumRadius - sphereSegmentDistance;
if (!isSphereShape1) {
normalWorld = -normalWorld;
}
}
else { // If the sphere center is on the capsule inner segment (degenerate case)
// We take any direction that is orthogonal to the inner capsule segment as a contact normal
// Capsule inner segment
Vector3 capsuleSegment = (capsuleSegB - capsuleSegA).getUnit();
Vector3 vec1(1, 0, 0);
Vector3 vec2(0, 1, 0);
// Get the vectors (among vec1 and vec2) that is the most orthogonal to the capsule inner segment (smallest absolute dot product)
decimal cosA1 = std::abs(capsuleSegment.x); // abs(vec1.dot(seg2))
decimal cosA2 = std::abs(capsuleSegment.y); // abs(vec2.dot(seg2))
penetrationDepth = sumRadius;
// We choose as a contact normal, any direction that is perpendicular to the inner capsule segment
Vector3 normalCapsuleSpace = cosA1 < cosA2 ? capsuleSegment.cross(vec1) : capsuleSegment.cross(vec2);
normalWorld = capsuleToWorldTransform.getOrientation() * normalCapsuleSpace;
// Compute the two local contact points
contactPointSphereLocal = sphereToCapsuleSpaceTransform.getInverse() * (sphereCenter + normalCapsuleSpace * sphereShape->getRadius());
contactPointCapsuleLocal = sphereCenter - normalCapsuleSpace * capsuleShape->getRadius();
}
// Create the contact info object
narrowPhaseInfo->addContactPoint(normalWorld, penetrationDepth,
isSphereShape1 ? contactPointSphereLocal : contactPointCapsuleLocal,
isSphereShape1 ? contactPointCapsuleLocal : contactPointSphereLocal);
}
return true;
}
return false;
}

70
src/collision/narrowphase/SphereVsCapsuleAlgorithm.h Normal file
View File

@ -0,0 +1,70 @@
/********************************************************************************
* 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_SPHERE_VS_CAPSULE_ALGORITHM_H
#define REACTPHYSICS3D_SPHERE_VS_CAPSULE_ALGORITHM_H
// Libraries
#include "body/Body.h"
#include "constraint/ContactPoint.h"
#include "NarrowPhaseAlgorithm.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Class SphereVsCapsuleAlgorithm
/**
* This class is used to compute the narrow-phase collision detection
* between a sphere collision shape and a capsule collision shape.
*/
class SphereVsCapsuleAlgorithm : public NarrowPhaseAlgorithm {
protected :
public :
// -------------------- Methods -------------------- //
/// Constructor
SphereVsCapsuleAlgorithm() = default;
/// Destructor
virtual ~SphereVsCapsuleAlgorithm() override = default;
/// Deleted copy-constructor
SphereVsCapsuleAlgorithm(const SphereVsCapsuleAlgorithm& algorithm) = delete;
/// Deleted assignment operator
SphereVsCapsuleAlgorithm& operator=(const SphereVsCapsuleAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between a sphere and a capsule
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override;
};
}
#endif

90
src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.cpp Normal file
View File

@ -0,0 +1,90 @@
/********************************************************************************
* 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 "SphereVsConvexPolyhedronAlgorithm.h"
#include "GJK/GJKAlgorithm.h"
#include "SAT/SATAlgorithm.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
// This technique is based on the "Robust Contact Creation for Physics Simulations" presentation
// by Dirk Gregorius.
bool SphereVsConvexPolyhedronAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
MemoryAllocator& memoryAllocator) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::CONVEX_POLYHEDRON ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::CONVEX_POLYHEDRON);
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE ||
narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
// Get the last frame collision info
LastFrameCollisionInfo* lastFrameCollisionInfo = narrowPhaseInfo->getLastFrameCollisionInfo();
// First, we run the GJK algorithm
GJKAlgorithm gjkAlgorithm;
#ifdef IS_PROFILING_ACTIVE
gjkAlgorithm.setProfiler(mProfiler);
#endif
GJKAlgorithm::GJKResult result = gjkAlgorithm.testCollision(narrowPhaseInfo, reportContacts);
lastFrameCollisionInfo->wasUsingGJK = true;
lastFrameCollisionInfo->wasUsingSAT = false;
// If we have found a contact point inside the margins (shallow penetration)
if (result == GJKAlgorithm::GJKResult::COLLIDE_IN_MARGIN) {
// Return true
return true;
}
// If we have overlap even without the margins (deep penetration)
if (result == GJKAlgorithm::GJKResult::INTERPENETRATE) {
// Run the SAT algorithm to find the separating axis and compute contact point
SATAlgorithm satAlgorithm(memoryAllocator);
#ifdef IS_PROFILING_ACTIVE
satAlgorithm.setProfiler(mProfiler);
#endif
bool isColliding = satAlgorithm.testCollisionSphereVsConvexPolyhedron(narrowPhaseInfo, reportContacts);
lastFrameCollisionInfo->wasUsingGJK = false;
lastFrameCollisionInfo->wasUsingSAT = true;
return isColliding;
}
return false;
}

70
src/collision/narrowphase/SphereVsConvexPolyhedronAlgorithm.h Normal file
View File

@ -0,0 +1,70 @@
/********************************************************************************
* 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_SPHERE_VS_CONVEX_POLYHEDRON_ALGORITHM_H
#define REACTPHYSICS3D_SPHERE_VS_CONVEX_POLYHEDRON_ALGORITHM_H
// Libraries
#include "body/Body.h"
#include "constraint/ContactPoint.h"
#include "NarrowPhaseAlgorithm.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Class SphereVsConvexPolyhedronAlgorithm
/**
* This class is used to compute the narrow-phase collision detection
* between a sphere and a convex polyhedron.
*/
class SphereVsConvexPolyhedronAlgorithm : public NarrowPhaseAlgorithm {
protected :
public :
// -------------------- Methods -------------------- //
/// Constructor
SphereVsConvexPolyhedronAlgorithm() = default;
/// Destructor
virtual ~SphereVsConvexPolyhedronAlgorithm() override = default;
/// Deleted copy-constructor
SphereVsConvexPolyhedronAlgorithm(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Deleted assignment operator
SphereVsConvexPolyhedronAlgorithm& operator=(const SphereVsConvexPolyhedronAlgorithm& algorithm) = delete;
/// Compute the narrow-phase collision detection between a sphere and a convex polyhedron
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override;
};
}
#endif

160
src/collision/narrowphase/SphereVsSphereAlgorithm.cpp Normal file → Executable file
View File

@ -1,70 +1,90 @@
/********************************************************************************
* 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 "SphereVsSphereAlgorithm.h"
#include "collision/shapes/SphereShape.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
void SphereVsSphereAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) {
// Get the sphere collision shapes
const SphereShape* sphereShape1 = static_cast<const SphereShape*>(shape1Info.collisionShape);
const SphereShape* sphereShape2 = static_cast<const SphereShape*>(shape2Info.collisionShape);
// Get the local-space to world-space transforms
const Transform& transform1 = shape1Info.shapeToWorldTransform;
const Transform& transform2 = shape2Info.shapeToWorldTransform;
// Compute the distance between the centers
Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare();
// Compute the sum of the radius
decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius();
// If the sphere collision shapes intersect
if (squaredDistanceBetweenCenters <= sumRadius * sumRadius) {
Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
Vector3 intersectionOnBody1 = sphereShape1->getRadius() *
centerSphere2InBody1LocalSpace.getUnit();
Vector3 intersectionOnBody2 = sphereShape2->getRadius() *
centerSphere1InBody2LocalSpace.getUnit();
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
// Create the contact info object
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, vectorBetweenCenters.getUnit(), penetrationDepth,
intersectionOnBody1, intersectionOnBody2);
// Notify about the new contact
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
}
}
/********************************************************************************
* 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 "SphereVsSphereAlgorithm.h"
#include "collision/shapes/SphereShape.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
bool SphereVsSphereAlgorithm::testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts,
MemoryAllocator& memoryAllocator) {
assert(narrowPhaseInfo->collisionShape1->getType() == CollisionShapeType::SPHERE);
assert(narrowPhaseInfo->collisionShape2->getType() == CollisionShapeType::SPHERE);
// Get the sphere collision shapes
const SphereShape* sphereShape1 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape1);
const SphereShape* sphereShape2 = static_cast<const SphereShape*>(narrowPhaseInfo->collisionShape2);
// Get the local-space to world-space transforms
const Transform& transform1 = narrowPhaseInfo->shape1ToWorldTransform;
const Transform& transform2 = narrowPhaseInfo->shape2ToWorldTransform;
// Compute the distance between the centers
Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
decimal squaredDistanceBetweenCenters = vectorBetweenCenters.lengthSquare();
// Compute the sum of the radius
decimal sumRadius = sphereShape1->getRadius() + sphereShape2->getRadius();
// If the sphere collision shapes intersect
if (squaredDistanceBetweenCenters < sumRadius * sumRadius) {
if (reportContacts) {
Vector3 centerSphere2InBody1LocalSpace = transform1.getInverse() * transform2.getPosition();
Vector3 centerSphere1InBody2LocalSpace = transform2.getInverse() * transform1.getPosition();
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
Vector3 intersectionOnBody1;
Vector3 intersectionOnBody2;
Vector3 normal;
// If the two sphere centers are not at the same position
if (squaredDistanceBetweenCenters > MACHINE_EPSILON) {
intersectionOnBody1 = sphereShape1->getRadius() * centerSphere2InBody1LocalSpace.getUnit();
intersectionOnBody2 = sphereShape2->getRadius() * centerSphere1InBody2LocalSpace.getUnit();
normal = vectorBetweenCenters.getUnit();
}
else { // If the sphere centers are at the same position (degenerate case)
// Take any contact normal direction
normal.setAllValues(0, 1, 0);
intersectionOnBody1 = sphereShape1->getRadius() * (transform1.getInverse().getOrientation() * normal);
intersectionOnBody2 = sphereShape2->getRadius() * (transform2.getInverse().getOrientation() * normal);
}
// Create the contact info object
narrowPhaseInfo->addContactPoint(normal, penetrationDepth, intersectionOnBody1, intersectionOnBody2);
}
return true;
}
return false;
}

142
src/collision/narrowphase/SphereVsSphereAlgorithm.h
View File

@ -1,72 +1,70 @@
/********************************************************************************
* 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_SPHERE_VS_SPHERE_ALGORITHM_H
#define REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H
// Libraries
#include "body/Body.h"
#include "constraint/ContactPoint.h"
#include "NarrowPhaseAlgorithm.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Class SphereVsSphereAlgorithm
/**
* This class is used to compute the narrow-phase collision detection
* between two sphere collision shapes.
*/
class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
protected :
public :
// -------------------- Methods -------------------- //
/// Constructor
SphereVsSphereAlgorithm() = default;
/// Destructor
virtual ~SphereVsSphereAlgorithm() override = default;
/// Deleted copy-constructor
SphereVsSphereAlgorithm(const SphereVsSphereAlgorithm& algorithm) = delete;
/// Deleted assignment operator
SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volume collide
virtual void testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) override;
};
}
#endif
/********************************************************************************
* 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_SPHERE_VS_SPHERE_ALGORITHM_H
#define REACTPHYSICS3D_SPHERE_VS_SPHERE_ALGORITHM_H
// Libraries
#include "body/Body.h"
#include "constraint/ContactPoint.h"
#include "NarrowPhaseAlgorithm.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Class SphereVsSphereAlgorithm
/**
* This class is used to compute the narrow-phase collision detection
* between two sphere collision shapes.
*/
class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
protected :
public :
// -------------------- Methods -------------------- //
/// Constructor
SphereVsSphereAlgorithm() = default;
/// Destructor
virtual ~SphereVsSphereAlgorithm() override = default;
/// Deleted copy-constructor
SphereVsSphereAlgorithm(const SphereVsSphereAlgorithm& algorithm) = delete;
/// Deleted assignment operator
SphereVsSphereAlgorithm& operator=(const SphereVsSphereAlgorithm& algorithm) = delete;
/// Compute a contact info if the two bounding volume collide
virtual bool testCollision(NarrowPhaseInfo* narrowPhaseInfo, bool reportContacts, MemoryAllocator& memoryAllocator) override;
};
}
#endif

58
src/collision/shapes/BoxShape.cpp
View File

@ -27,6 +27,7 @@
#include "BoxShape.h"
#include "collision/ProxyShape.h"
#include "configuration.h"
#include "memory/MemoryManager.h"
#include <vector>
#include <cassert>
@ -35,13 +36,49 @@ using namespace reactphysics3d;
// Constructor
/**
* @param extent The vector with the three extents of the box (in meters)
* @param margin The collision margin (in meters) around the collision shape
*/
BoxShape::BoxShape(const Vector3& extent, decimal margin)
: ConvexShape(CollisionShapeType::BOX, margin), mExtent(extent - Vector3(margin, margin, margin)) {
assert(extent.x > decimal(0.0) && extent.x > margin);
assert(extent.y > decimal(0.0) && extent.y > margin);
assert(extent.z > decimal(0.0) && extent.z > margin);
BoxShape::BoxShape(const Vector3& extent)
: ConvexPolyhedronShape(CollisionShapeName::BOX), mExtent(extent),
mHalfEdgeStructure(MemoryManager::getBaseAllocator(), 6, 8, 24) {
assert(extent.x > decimal(0.0));
assert(extent.y > decimal(0.0));
assert(extent.z > decimal(0.0));
// Vertices
mHalfEdgeStructure.addVertex(0);
mHalfEdgeStructure.addVertex(1);
mHalfEdgeStructure.addVertex(2);
mHalfEdgeStructure.addVertex(3);
mHalfEdgeStructure.addVertex(4);
mHalfEdgeStructure.addVertex(5);
mHalfEdgeStructure.addVertex(6);
mHalfEdgeStructure.addVertex(7);
MemoryAllocator& allocator = MemoryManager::getBaseAllocator();
// Faces
List<uint> face0(allocator, 4);
face0.add(0); face0.add(1); face0.add(2); face0.add(3);
List<uint> face1(allocator, 4);
face1.add(1); face1.add(5); face1.add(6); face1.add(2);
List<uint> face2(allocator, 4);
face2.add(4); face2.add(7); face2.add(6); face2.add(5);
List<uint> face3(allocator, 4);
face3.add(4); face3.add(0); face3.add(3); face3.add(7);
List<uint> face4(allocator, 4);
face4.add(4); face4.add(5); face4.add(1); face4.add(0);
List<uint> face5(allocator, 4);
face5.add(2); face5.add(6); face5.add(7); face5.add(3);
mHalfEdgeStructure.addFace(face0);
mHalfEdgeStructure.addFace(face1);
mHalfEdgeStructure.addFace(face2);
mHalfEdgeStructure.addFace(face3);
mHalfEdgeStructure.addFace(face4);
mHalfEdgeStructure.addFace(face5);
mHalfEdgeStructure.init();
}
// Return the local inertia tensor of the collision shape
@ -52,17 +89,16 @@ BoxShape::BoxShape(const Vector3& extent, decimal margin)
*/
void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
decimal factor = (decimal(1.0) / decimal(3.0)) * mass;
Vector3 realExtent = mExtent + Vector3(mMargin, mMargin, mMargin);
decimal xSquare = realExtent.x * realExtent.x;
decimal ySquare = realExtent.y * realExtent.y;
decimal zSquare = realExtent.z * realExtent.z;
decimal xSquare = mExtent.x * mExtent.x;
decimal ySquare = mExtent.y * mExtent.y;
decimal zSquare = mExtent.z * mExtent.z;
tensor.setAllValues(factor * (ySquare + zSquare), 0.0, 0.0,
0.0, factor * (xSquare + zSquare), 0.0,
0.0, 0.0, factor * (xSquare + ySquare));
}
// Raycast method with feedback information
bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const {
Vector3 rayDirection = ray.point2 - ray.point1;
decimal tMin = DECIMAL_SMALLEST;

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

@ -28,10 +28,10 @@
// Libraries
#include <cfloat>
#include "ConvexShape.h"
#include "ConvexPolyhedronShape.h"
#include "body/CollisionBody.h"
#include "mathematics/mathematics.h"
#include "memory/DefaultAllocator.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@ -41,16 +41,9 @@ namespace reactphysics3d {
* This class represents a 3D box shape. Those axis are unit length.
* The three extents are half-widths of the box along the three
* axis x, y, z local axis. The "transform" of the corresponding
* rigid body will give an orientation and a position to the box. This
* collision shape uses an extra margin distance around it for collision
* detection purpose. The default margin is 4cm (if your units are meters,
* which is recommended). In case, you want to simulate small objects
* (smaller than the margin distance), you might want to reduce the margin by
* specifying your own margin distance using the "margin" parameter in the
* constructor of the box shape. Otherwise, it is recommended to use the
* default margin distance by not using the "margin" parameter in the constructor.
* body will give an orientation and a position to the box.
*/
class BoxShape : public ConvexShape {
class BoxShape : public ConvexPolyhedronShape {
protected :
@ -59,17 +52,19 @@ class BoxShape : public ConvexShape {
/// Extent sizes of the box in the x, y and z direction
Vector3 mExtent;
/// Half-edge structure of the polyhedron
HalfEdgeStructure mHalfEdgeStructure;
// -------------------- Methods -------------------- //
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const override;
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
@ -79,7 +74,7 @@ class BoxShape : public ConvexShape {
// -------------------- Methods -------------------- //
/// Constructor
BoxShape(const Vector3& extent, decimal margin = OBJECT_MARGIN);
BoxShape(const Vector3& extent);
/// Destructor
virtual ~BoxShape() override = default;
@ -99,11 +94,35 @@ 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 override;
/// 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 override;
/// Return the number of faces of the polyhedron
virtual uint getNbFaces() const override;
/// Return a given face of the polyhedron
virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const override;
/// Return the number of vertices of the polyhedron
virtual uint getNbVertices() const override;
/// Return a given vertex of the polyhedron
virtual HalfEdgeStructure::Vertex getVertex(uint vertexIndex) const override;
/// Return the number of half-edges of the polyhedron
virtual uint getNbHalfEdges() const override;
/// Return a given half-edge of the polyhedron
virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const override;
/// Return the position of a given vertex
virtual Vector3 getVertexPosition(uint vertexIndex) const override;
/// Return the normal vector of a given face of the polyhedron
virtual Vector3 getFaceNormal(uint faceIndex) const override;
/// Return the centroid of the polyhedron
virtual Vector3 getCentroid() const override;
};
// Return the extents of the box
@ -111,7 +130,7 @@ class BoxShape : public ConvexShape {
* @return The vector with the three extents of the box shape (in meters)
*/
inline Vector3 BoxShape::getExtent() const {
return mExtent + Vector3(mMargin, mMargin, mMargin);
return mExtent;
}
// Set the scaling vector of the collision shape
@ -131,29 +150,23 @@ inline void BoxShape::setLocalScaling(const Vector3& scaling) {
inline void BoxShape::getLocalBounds(Vector3& min, Vector3& max) const {
// Maximum bounds
max = mExtent + Vector3(mMargin, mMargin, mMargin);
max = mExtent;
// Minimum bounds
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);
}
// Return a local support point in a given direction without the objec margin
inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
// Return a local support point in a given direction without the object margin
inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const {
return Vector3(direction.x < 0.0 ? -mExtent.x : mExtent.x,
direction.y < 0.0 ? -mExtent.y : mExtent.y,
direction.z < 0.0 ? -mExtent.z : mExtent.z);
return Vector3(direction.x < decimal(0.0) ? -mExtent.x : mExtent.x,
direction.y < decimal(0.0) ? -mExtent.y : mExtent.y,
direction.z < decimal(0.0) ? -mExtent.z : mExtent.z);
}
// Return true if a point is inside the collision shape
@ -163,6 +176,78 @@ inline bool BoxShape::testPointInside(const Vector3& localPoint, ProxyShape* pro
localPoint.z < mExtent[2] && localPoint.z > -mExtent[2]);
}
// Return the number of faces of the polyhedron
inline uint BoxShape::getNbFaces() const {
return 6;
}
// Return a given face of the polyhedron
inline const HalfEdgeStructure::Face& BoxShape::getFace(uint faceIndex) const {
assert(faceIndex < mHalfEdgeStructure.getNbFaces());
return mHalfEdgeStructure.getFace(faceIndex);
}
// Return the number of vertices of the polyhedron
inline uint BoxShape::getNbVertices() const {
return 8;
}
// Return a given vertex of the polyhedron
inline HalfEdgeStructure::Vertex BoxShape::getVertex(uint vertexIndex) const {
assert(vertexIndex < getNbVertices());
return mHalfEdgeStructure.getVertex(vertexIndex);
}
// Return the position of a given vertex
inline Vector3 BoxShape::getVertexPosition(uint vertexIndex) const {
assert(vertexIndex < getNbVertices());
Vector3 extent = getExtent();
switch(vertexIndex) {
case 0: return Vector3(-extent.x, -extent.y, extent.z);
case 1: return Vector3(extent.x, -extent.y, extent.z);
case 2: return Vector3(extent.x, extent.y, extent.z);
case 3: return Vector3(-extent.x, extent.y, extent.z);
case 4: return Vector3(-extent.x, -extent.y, -extent.z);
case 5: return Vector3(extent.x, -extent.y, -extent.z);
case 6: return Vector3(extent.x, extent.y, -extent.z);
case 7: return Vector3(-extent.x, extent.y, -extent.z);
}
}
// Return the normal vector of a given face of the polyhedron
inline Vector3 BoxShape::getFaceNormal(uint faceIndex) const {
assert(faceIndex < getNbFaces());
switch(faceIndex) {
case 0: return Vector3(0, 0, 1);
case 1: return Vector3(1, 0, 0);
case 2: return Vector3(0, 0, -1);
case 3: return Vector3(-1, 0, 0);
case 4: return Vector3(0, -1, 0);
case 5: return Vector3(0, 1, 0);
}
assert(false);
}
// Return the centroid of the box
inline Vector3 BoxShape::getCentroid() const {
return Vector3::zero();
}
// Return the number of half-edges of the polyhedron
inline uint BoxShape::getNbHalfEdges() const {
return 24;
}
// Return a given half-edge of the polyhedron
inline const HalfEdgeStructure::Edge& BoxShape::getHalfEdge(uint edgeIndex) const {
assert(edgeIndex < getNbHalfEdges());
return mHalfEdgeStructure.getHalfEdge(edgeIndex);
}
}
#endif

4
src/collision/shapes/CapsuleShape.cpp
View File

@ -37,7 +37,7 @@ using namespace reactphysics3d;
* @param height The height of the capsule (in meters)
*/
CapsuleShape::CapsuleShape(decimal radius, decimal height)
: ConvexShape(CollisionShapeType::CAPSULE, radius), mHalfHeight(height * decimal(0.5)) {
: ConvexShape(CollisionShapeName::CAPSULE, CollisionShapeType::CAPSULE, radius), mHalfHeight(height * decimal(0.5)) {
assert(radius > decimal(0.0));
assert(height > decimal(0.0));
}
@ -85,7 +85,7 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
}
// Raycast method with feedback information
bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const {
const Vector3 n = ray.point2 - ray.point1;

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

@ -56,14 +56,13 @@ class CapsuleShape : public ConvexShape {
// -------------------- Methods -------------------- //
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const override;
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override;
/// Raycasting method between a ray one of the two spheres end cap of the capsule
bool raycastWithSphereEndCap(const Vector3& point1, const Vector3& point2,
@ -102,7 +101,7 @@ class CapsuleShape : public ConvexShape {
virtual void getLocalBounds(Vector3& min, Vector3& max) const override;
/// Return true if the collision shape is a polyhedron
virtual bool isPolyhedron() const;
virtual bool isPolyhedron() const override;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override;
@ -169,8 +168,7 @@ inline bool CapsuleShape::isPolyhedron() const {
/// Therefore, in this method, we compute the support points of both top and bottom spheres of
/// the capsule and return the point with the maximum dot product with the direction vector. Note
/// that the object margin is implicitly the radius and height of the capsule.
inline Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
inline Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const {
// Support point top sphere
decimal dotProductTop = mHalfHeight * direction.y;

52
src/collision/shapes/CollisionShape.cpp
View File

@ -32,39 +32,57 @@
using namespace reactphysics3d;
// Constructor
CollisionShape::CollisionShape(CollisionShapeType type) : mType(type), mScaling(1.0, 1.0, 1.0) {
CollisionShape::CollisionShape(CollisionShapeName name, CollisionShapeType type)
: mType(type), mName(name), mScaling(1.0, 1.0, 1.0), mId(0) {
}
// Compute the world-space AABB of the collision shape given a transform
// Compute the world-space AABB of the collision shape given a transform from shape
// local-space to world-space. The technique is described in the book Real-Time Collision
// Detection by Christer Ericson.
/**
* @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape
* computed in world-space coordinates
* @param transform Transform used to compute the AABB of the collision shape
* @param transform Transform from shape local-space to world-space used to compute
* the AABB of the collision shape
*/
void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const {
PROFILE("CollisionShape::computeAABB()");
PROFILE("CollisionShape::computeAABB()", mProfiler);
// Get the local bounds in x,y and z direction
Vector3 minBounds;
Vector3 maxBounds;
getLocalBounds(minBounds, maxBounds);
// Rotate the local bounds according to the orientation of the body
Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsoluteMatrix();
Vector3 worldMinBounds(worldAxis.getColumn(0).dot(minBounds),
worldAxis.getColumn(1).dot(minBounds),
worldAxis.getColumn(2).dot(minBounds));
Vector3 worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds),
worldAxis.getColumn(1).dot(maxBounds),
worldAxis.getColumn(2).dot(maxBounds));
const Vector3 translation = transform.getPosition();
Matrix3x3 matrix = transform.getOrientation().getMatrix();
Vector3 resultMin;
Vector3 resultMax;
// Compute the minimum and maximum coordinates of the rotated extents
Vector3 minCoordinates = transform.getPosition() + worldMinBounds;
Vector3 maxCoordinates = transform.getPosition() + worldMaxBounds;
// For each of the three axis
for (int i=0; i<3; i++) {
// Add translation component
resultMin[i] = translation[i];
resultMax[i] = translation[i];
for (int j=0; j<3; j++) {
decimal e = matrix[i][j] * minBounds[j];
decimal f = matrix[i][j] * maxBounds[j];
if (e < f) {
resultMin[i] += e;
resultMax[i] += f;
}
else {
resultMin[i] += f;
resultMax[i] += e;
}
}
}
// Update the AABB with the new minimum and maximum coordinates
aabb.setMin(minCoordinates);
aabb.setMax(maxCoordinates);
aabb.setMin(resultMin);
aabb.setMax(resultMax);
}

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

@ -34,15 +34,18 @@
#include "mathematics/Ray.h"
#include "AABB.h"
#include "collision/RaycastInfo.h"
#include "memory/MemoryAllocator.h"
#include "memory/PoolAllocator.h"
#include "engine/Profiler.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
/// Type of the collision shape
enum class CollisionShapeType {TRIANGLE, BOX, SPHERE, CONE, CYLINDER,
CAPSULE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD};
const int NB_COLLISION_SHAPE_TYPES = 9;
/// Type of collision shapes
enum class CollisionShapeType {SPHERE, CAPSULE, CONVEX_POLYHEDRON, CONCAVE_SHAPE};
const int NB_COLLISION_SHAPE_TYPES = 4;
/// Names of collision shapes
enum class CollisionShapeName { TRIANGLE, SPHERE, CAPSULE, BOX, CONVEX_MESH, TRIANGLE_MESH, HEIGHTFIELD };
// Declarations
class ProxyShape;
@ -62,8 +65,21 @@ class CollisionShape {
/// Type of the collision shape
CollisionShapeType mType;
/// Name of the colision shape
CollisionShapeName mName;
/// Scaling vector of the collision shape
Vector3 mScaling;
/// Unique identifier of the shape inside an overlapping pair
uint mId;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
// -------------------- Methods -------------------- //
@ -71,7 +87,7 @@ class CollisionShape {
virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const=0;
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const=0;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const = 0;
@ -81,7 +97,7 @@ class CollisionShape {
// -------------------- Methods -------------------- //
/// Constructor
CollisionShape(CollisionShapeType type);
CollisionShape(CollisionShapeName name, CollisionShapeType type);
/// Destructor
virtual ~CollisionShape() = default;
@ -92,7 +108,10 @@ class CollisionShape {
/// Deleted assignment operator
CollisionShape& operator=(const CollisionShape& shape) = delete;
/// Return the type of the collision shapes
/// Return the name of the collision shape
CollisionShapeName getName() const;
/// Return the type of the collision shape
CollisionShapeType getType() const;
/// Return true if the collision shape is convex, false if it is concave
@ -105,23 +124,26 @@ class CollisionShape {
virtual void getLocalBounds(Vector3& min, Vector3& max) const=0;
/// Return the scaling vector of the collision shape
Vector3 getScaling() const;
Vector3 getLocalScaling() const;
/// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
/// Return the id of the shape
uint getId() const;
/// Return the local inertia tensor of the collision shapes
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0;
/// Compute the world-space AABB of the collision shape given a transform
virtual void computeAABB(AABB& aabb, const Transform& transform) const;
/// Return true if the collision shape type is a convex shape
static bool isConvex(CollisionShapeType shapeType);
#ifdef IS_PROFILING_ACTIVE
/// Return the maximum number of contact manifolds in an overlapping pair given two shape types
static int computeNbMaxContactManifolds(CollisionShapeType shapeType1,
CollisionShapeType shapeType2);
/// Set the profiler
virtual void setProfiler(Profiler* profiler);
#endif
// -------------------- Friendship -------------------- //
@ -129,21 +151,24 @@ class CollisionShape {
friend class CollisionWorld;
};
// Return the name of the collision shape
/**
* @return The name of the collision shape (box, sphere, triangle, ...)
*/
inline CollisionShapeName CollisionShape::getName() const {
return mName;
}
// Return the type of the collision shape
/**
* @return The type of the collision shape (box, sphere, cylinder, ...)
* @return The type of the collision shape (sphere, capsule, convex polyhedron, concave mesh)
*/
inline CollisionShapeType CollisionShape::getType() const {
return mType;
}
// Return true if the collision shape type is a convex shape
inline bool CollisionShape::isConvex(CollisionShapeType shapeType) {
return shapeType != CollisionShapeType::CONCAVE_MESH && shapeType != CollisionShapeType::HEIGHTFIELD;
}
// Return the scaling vector of the collision shape
inline Vector3 CollisionShape::getScaling() const {
inline Vector3 CollisionShape::getLocalScaling() const {
return mScaling;
}
@ -152,19 +177,21 @@ inline void CollisionShape::setLocalScaling(const Vector3& scaling) {
mScaling = scaling;
}
// Return the maximum number of contact manifolds allowed in an overlapping
// pair wit the given two collision shape types
inline int CollisionShape::computeNbMaxContactManifolds(CollisionShapeType shapeType1,
CollisionShapeType shapeType2) {
// If both shapes are convex
if (isConvex(shapeType1) && isConvex(shapeType2)) {
return NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE;
} // If there is at least one concave shape
else {
return NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE;
}
// Return the id of the shape
inline uint CollisionShape::getId() const {
return mId;
}
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void CollisionShape::setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
#endif
}
#endif

158
src/collision/shapes/ConcaveMeshShape.cpp
View File

@ -30,7 +30,7 @@ using namespace reactphysics3d;
// Constructor
ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh)
: ConcaveShape(CollisionShapeType::CONCAVE_MESH) {
: ConcaveShape(CollisionShapeName::TRIANGLE_MESH) {
mTriangleMesh = triangleMesh;
mRaycastTestType = TriangleRaycastSide::FRONT;
@ -49,55 +49,21 @@ void ConcaveMeshShape::initBVHTree() {
// Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart);
TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
int vertexStride = triangleVertexArray->getVerticesStride();
int indexStride = triangleVertexArray->getIndicesStride();
// For each triangle of the concave mesh
for (uint triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) {
void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
Vector3 trianglePoints[3];
// For each vertex of the triangle
for (int k=0; k < 3; k++) {
// Get the triangle vertices
triangleVertexArray->getTriangleVertices(triangleIndex, trianglePoints);
// Get the index of the current vertex in the triangle
int vertexIndex = 0;
if (indexType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
vertexIndex = ((uint*)vertexIndexPointer)[k];
}
else if (indexType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
}
else {
assert(false);
}
// Get the vertices components of the triangle
if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) {
const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x;
trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y;
trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z;
}
else if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) {
const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
trianglePoints[k][0] = decimal(vertices[0]) * mScaling.x;
trianglePoints[k][1] = decimal(vertices[1]) * mScaling.y;
trianglePoints[k][2] = decimal(vertices[2]) * mScaling.z;
}
else {
assert(false);
}
}
// Apply the scaling factor to the vertices
trianglePoints[0] *= mScaling.x;
trianglePoints[1] *= mScaling.y;
trianglePoints[2] *= mScaling.z;
// Create the AABB for the triangle
AABB aabb = AABB::createAABBForTriangle(trianglePoints);
aabb.inflate(mTriangleMargin, mTriangleMargin, mTriangleMargin);
// Add the AABB with the index of the triangle into the dynamic AABB tree
mDynamicAABBTree.addObject(aabb, subPart, triangleIndex);
@ -106,56 +72,32 @@ void ConcaveMeshShape::initBVHTree() {
}
// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
// given the start vertex index pointer of the triangle
void ConcaveMeshShape::getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex,
Vector3* outTriangleVertices) const {
void ConcaveMeshShape::getTriangleVertices(uint subPart, uint triangleIndex,
Vector3* outTriangleVertices) const {
// Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart);
TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
int vertexStride = triangleVertexArray->getVerticesStride();
int indexStride = triangleVertexArray->getIndicesStride();
// Get the vertices coordinates of the triangle
triangleVertexArray->getTriangleVertices(triangleIndex, outTriangleVertices);
void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
// For each vertex of the triangle
for (int k=0; k < 3; k++) {
// Get the index of the current vertex in the triangle
int vertexIndex = 0;
if (indexType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
vertexIndex = ((uint*)vertexIndexPointer)[k];
}
else if (indexType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
}
else {
assert(false);
}
// Get the vertices components of the triangle
if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) {
const float* vertices = (float*)(verticesStart + vertexIndex * vertexStride);
outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x;
outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y;
outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z;
}
else if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) {
const double* vertices = (double*)(verticesStart + vertexIndex * vertexStride);
outTriangleVertices[k][0] = decimal(vertices[0]) * mScaling.x;
outTriangleVertices[k][1] = decimal(vertices[1]) * mScaling.y;
outTriangleVertices[k][2] = decimal(vertices[2]) * mScaling.z;
}
else {
assert(false);
}
}
// Apply the scaling factor to the vertices
outTriangleVertices[0] *= mScaling.x;
outTriangleVertices[1] *= mScaling.y;
outTriangleVertices[2] *= mScaling.z;
}
// Return the three vertex normals (in the array outVerticesNormals) of a triangle
void ConcaveMeshShape::getTriangleVerticesNormals(uint subPart, uint triangleIndex, Vector3* outVerticesNormals) const {
// Get the triangle vertex array of the current sub-part
TriangleVertexArray* triangleVertexArray = mTriangleMesh->getSubpart(subPart);
// Get the vertices normals of the triangle
triangleVertexArray->getTriangleVerticesNormals(triangleIndex, outVerticesNormals);
}
// Use a callback method on all triangles of the concave shape inside a given AABB
void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const {
@ -169,12 +111,19 @@ void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB&
// Raycast method with feedback information
/// Note that only the first triangle hit by the ray in the mesh will be returned, even if
/// the ray hits many triangles.
bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const {
PROFILE("ConcaveMeshShape::raycast()");
PROFILE("ConcaveMeshShape::raycast()", mProfiler);
// Create the callback object that will compute ray casting against triangles
ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray);
ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray, allocator);
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
raycastCallback.setProfiler(mProfiler);
#endif
// Ask the Dynamic AABB Tree to report all AABB nodes that are hit by the ray.
// The raycastCallback object will then compute ray casting against the triangles
@ -186,6 +135,22 @@ bool ConcaveMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxySh
return raycastCallback.getIsHit();
}
// Compute the shape Id for a given triangle of the mesh
uint ConcaveMeshShape::computeTriangleShapeId(uint subPart, uint triangleIndex) const {
uint shapeId = 0;
uint i=0;
while (i < subPart) {
shapeId += mTriangleMesh->getSubpart(i)->getNbTriangles();
i++;
}
return shapeId + triangleIndex;
}
// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree
decimal ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) {
@ -208,16 +173,26 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
// Get the triangle vertices for this node from the concave mesh shape
Vector3 trianglePoints[3];
mConcaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
mConcaveMeshShape.getTriangleVertices(data[0], data[1], trianglePoints);
// Get the vertices normals of the triangle
Vector3 verticesNormals[3];
mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals);
// Create a triangle collision shape
decimal margin = mConcaveMeshShape.getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
TriangleShape triangleShape(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1]), mAllocator);
triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType());
#ifdef IS_PROFILING_ACTIVE
// Set the profiler to the triangle shape
triangleShape.setProfiler(mProfiler);
#endif
// Ray casting test against the collision shape
RaycastInfo raycastInfo;
bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape);
bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape, mAllocator);
// If the ray hit the collision shape
if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) {
@ -235,5 +210,6 @@ void ConcaveMeshRaycastCallback::raycastTriangles() {
smallestHitFraction = raycastInfo.hitFraction;
mIsHit = true;
}
}
}

66
src/collision/shapes/ConcaveMeshShape.h
View File

@ -77,14 +77,22 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
RaycastInfo& mRaycastInfo;
const Ray& mRay;
bool mIsHit;
MemoryAllocator& mAllocator;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
public:
// Constructor
ConcaveMeshRaycastCallback(const DynamicAABBTree& dynamicAABBTree, const ConcaveMeshShape& concaveMeshShape,
ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray)
ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray, MemoryAllocator& allocator)
: mDynamicAABBTree(dynamicAABBTree), mConcaveMeshShape(concaveMeshShape), mProxyShape(proxyShape),
mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false) {
mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false), mAllocator(allocator) {
}
@ -98,12 +106,21 @@ class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
bool getIsHit() const {
return mIsHit;
}
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
void setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
#endif
};
// Class ConcaveMeshShape
/**
* This class represents a static concave mesh shape. Note that collision detection
* with a concave mesh shape can be very expensive. You should use only use
* with a concave mesh shape can be very expensive. You should only use
* this shape for a static mesh.
*/
class ConcaveMeshShape : public ConcaveShape {
@ -118,10 +135,14 @@ class ConcaveMeshShape : public ConcaveShape {
/// Dynamic AABB tree to accelerate collision with the triangles
DynamicAABBTree mDynamicAABBTree;
/// Array with computed vertices normals for each TriangleVertexArray of the triangle mesh (only
/// if the user did not provide its own vertices normals)
Vector3** mComputedVerticesNormals;
// -------------------- Methods -------------------- //
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
@ -130,9 +151,13 @@ class ConcaveMeshShape : public ConcaveShape {
void initBVHTree();
/// Return the three vertices coordinates (in the array outTriangleVertices) of a triangle
/// given the start vertex index pointer of the triangle.
void getTriangleVerticesWithIndexPointer(int32 subPart, int32 triangleIndex,
Vector3* outTriangleVertices) const;
void getTriangleVertices(uint subPart, uint triangleIndex, Vector3* outTriangleVertices) const;
/// Return the three vertex normals (in the array outVerticesNormals) of a triangle
void getTriangleVerticesNormals(uint subPart, uint triangleIndex, Vector3* outVerticesNormals) const;
/// Compute the shape Id for a given triangle of the mesh
uint computeTriangleShapeId(uint subPart, uint triangleIndex) const;
public:
@ -160,6 +185,13 @@ class ConcaveMeshShape : public ConcaveShape {
/// Use a callback method on all triangles of the concave shape inside a given AABB
virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const override;
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
virtual void setProfiler(Profiler* profiler) override;
#endif
// ---------- Friendship ----------- //
friend class ConvexTriangleAABBOverlapCallback;
@ -224,12 +256,28 @@ inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId)
// Get the triangle vertices for this node from the concave mesh shape
Vector3 trianglePoints[3];
mConcaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
mConcaveMeshShape.getTriangleVertices(data[0], data[1], trianglePoints);
// Get the vertices normals of the triangle
Vector3 verticesNormals[3];
mConcaveMeshShape.getTriangleVerticesNormals(data[0], data[1], verticesNormals);
// Call the callback to test narrow-phase collision with this triangle
mTriangleTestCallback.testTriangle(trianglePoints);
mTriangleTestCallback.testTriangle(trianglePoints, verticesNormals, mConcaveMeshShape.computeTriangleShapeId(data[0], data[1]));
}
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
inline void ConcaveMeshShape::setProfiler(Profiler* profiler) {
CollisionShape::setProfiler(profiler);
mDynamicAABBTree.setProfiler(profiler);
}
#endif
}
#endif

5
src/collision/shapes/ConcaveShape.cpp
View File

@ -31,8 +31,7 @@
using namespace reactphysics3d;
// Constructor
ConcaveShape::ConcaveShape(CollisionShapeType type)
: CollisionShape(type), mIsSmoothMeshCollisionEnabled(false),
mTriangleMargin(0), mRaycastTestType(TriangleRaycastSide::FRONT) {
ConcaveShape::ConcaveShape(CollisionShapeName name)
: CollisionShape(name, CollisionShapeType::CONCAVE_SHAPE), mRaycastTestType(TriangleRaycastSide::FRONT) {
}

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

@ -46,7 +46,7 @@ class TriangleCallback {
virtual ~TriangleCallback() = default;
/// Report a triangle
virtual void testTriangle(const Vector3* trianglePoints)=0;
virtual void testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId)=0;
};
@ -62,12 +62,6 @@ class ConcaveShape : public CollisionShape {
// -------------------- Attributes -------------------- //
/// True if the smooth mesh collision algorithm is enabled
bool mIsSmoothMeshCollisionEnabled;
// Margin use for collision detection for each triangle
decimal mTriangleMargin;
/// Raycast test type for the triangle (front, back, front-back)
TriangleRaycastSide mRaycastTestType;
@ -81,7 +75,7 @@ class ConcaveShape : public CollisionShape {
// -------------------- Methods -------------------- //
/// Constructor
ConcaveShape(CollisionShapeType type);
ConcaveShape(CollisionShapeName name);
/// Destructor
virtual ~ConcaveShape() override = default;
@ -92,9 +86,6 @@ class ConcaveShape : public CollisionShape {
/// Deleted assignment operator
ConcaveShape& operator=(const ConcaveShape& shape) = delete;
/// Return the triangle margin
decimal getTriangleMargin() const;
/// Return the raycast test type (front, back, front-back)
TriangleRaycastSide getRaycastTestType() const;
@ -105,23 +96,12 @@ class ConcaveShape : public CollisionShape {
virtual bool isConvex() const override;
/// Return true if the collision shape is a polyhedron
virtual bool isPolyhedron() const;
virtual bool isPolyhedron() const override;
/// 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;
/// Return true if the smooth mesh collision is enabled
bool getIsSmoothMeshCollisionEnabled() const;
/// Enable/disable the smooth mesh collision algorithm
void setIsSmoothMeshCollisionEnabled(bool isEnabled);
};
// Return the triangle margin
inline decimal ConcaveShape::getTriangleMargin() const {
return mTriangleMargin;
}
// Return true if the collision shape is convex, false if it is concave
inline bool ConcaveShape::isConvex() const {
return false;
@ -137,19 +117,6 @@ inline bool ConcaveShape::testPointInside(const Vector3& localPoint, ProxyShape*
return false;
}
// Return true if the smooth mesh collision is enabled
inline bool ConcaveShape::getIsSmoothMeshCollisionEnabled() const {
return mIsSmoothMeshCollisionEnabled;
}
// Enable/disable the smooth mesh collision algorithm
/// Smooth mesh collision is used to avoid collisions against some internal edges
/// of the triangle mesh. If it is enabled, collsions with the mesh will be smoother
/// but collisions computation is a bit more expensive.
inline void ConcaveShape::setIsSmoothMeshCollisionEnabled(bool isEnabled) {
mIsSmoothMeshCollisionEnabled = isEnabled;
}
// Return the raycast test type (front, back, front-back)
inline TriangleRaycastSide ConcaveShape::getRaycastTestType() const {
return mRaycastTestType;

209
src/collision/shapes/ConeShape.cpp
View File

@ -1,209 +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 <complex>
#include "configuration.h"
#include "ConeShape.h"
#include "collision/ProxyShape.h"
using namespace reactphysics3d;
// Constructor
/**
* @param radius Radius of the cone (in meters)
* @param height Height of the cone (in meters)
* @param margin Collision margin (in meters) around the collision shape
*/
ConeShape::ConeShape(decimal radius, decimal height, decimal margin)
: ConvexShape(CollisionShapeType::CONE, margin), mRadius(radius), mHalfHeight(height * decimal(0.5)) {
assert(mRadius > decimal(0.0));
assert(mHalfHeight > decimal(0.0));
// Compute the sine of the semi-angle at the apex point
mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height));
}
// Return a local support point in a given direction without the object margin
Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
const Vector3& v = direction;
decimal sinThetaTimesLengthV = mSinTheta * v.length();
Vector3 supportPoint;
if (v.y > sinThetaTimesLengthV) {
supportPoint = Vector3(0.0, mHalfHeight, 0.0);
}
else {
decimal projectedLength = sqrt(v.x * v.x + v.z * v.z);
if (projectedLength > MACHINE_EPSILON) {
decimal d = mRadius / projectedLength;
supportPoint = Vector3(v.x * d, -mHalfHeight, v.z * d);
}
else {
supportPoint = Vector3(0.0, -mHalfHeight, 0.0);
}
}
return supportPoint;
}
// Raycast method with feedback information
// This implementation is based on the technique described by David Eberly in the article
// "Intersection of a Line and a Cone" that can be found at
// http://www.geometrictools.com/Documentation/IntersectionLineCone.pdf
bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
const Vector3 r = ray.point2 - ray.point1;
const decimal epsilon = decimal(0.00001);
Vector3 V(0, mHalfHeight, 0);
Vector3 centerBase(0, -mHalfHeight, 0);
Vector3 axis(0, decimal(-1.0), 0);
decimal heightSquare = decimal(4.0) * mHalfHeight * mHalfHeight;
decimal cosThetaSquare = heightSquare / (heightSquare + mRadius * mRadius);
decimal factor = decimal(1.0) - cosThetaSquare;
Vector3 delta = ray.point1 - V;
decimal c0 = -cosThetaSquare * delta.x * delta.x + factor * delta.y * delta.y -
cosThetaSquare * delta.z * delta.z;
decimal c1 = -cosThetaSquare * delta.x * r.x + factor * delta.y * r.y - cosThetaSquare * delta.z * r.z;
decimal c2 = -cosThetaSquare * r.x * r.x + factor * r.y * r.y - cosThetaSquare * r.z * r.z;
decimal tHit[] = {decimal(-1.0), decimal(-1.0), decimal(-1.0)};
Vector3 localHitPoint[3];
Vector3 localNormal[3];
// If c2 is different from zero
if (std::abs(c2) > MACHINE_EPSILON) {
decimal gamma = c1 * c1 - c0 * c2;
// If there is no real roots in the quadratic equation
if (gamma < decimal(0.0)) {
return false;
}
else if (gamma > decimal(0.0)) { // The equation has two real roots
// Compute two intersections
decimal sqrRoot = std::sqrt(gamma);
tHit[0] = (-c1 - sqrRoot) / c2;
tHit[1] = (-c1 + sqrRoot) / c2;
}
else { // If the equation has a single real root
// Compute the intersection
tHit[0] = -c1 / c2;
}
}
else { // If c2 == 0
// If c2 = 0 and c1 != 0
if (std::abs(c1) > MACHINE_EPSILON) {
tHit[0] = -c0 / (decimal(2.0) * c1);
}
else { // If c2 = c1 = 0
// If c0 is different from zero, no solution and if c0 = 0, we have a
// degenerate case, the whole ray is contained in the cone side
// but we return no hit in this case
return false;
}
}
// If the origin of the ray is inside the cone, we return no hit
if (testPointInside(ray.point1, nullptr)) return false;
localHitPoint[0] = ray.point1 + tHit[0] * r;
localHitPoint[1] = ray.point1 + tHit[1] * r;
// Only keep hit points in one side of the double cone (the cone we are interested in)
if (axis.dot(localHitPoint[0] - V) < decimal(0.0)) {
tHit[0] = decimal(-1.0);
}
if (axis.dot(localHitPoint[1] - V) < decimal(0.0)) {
tHit[1] = decimal(-1.0);
}
// Only keep hit points that are within the correct height of the cone
if (localHitPoint[0].y < decimal(-mHalfHeight)) {
tHit[0] = decimal(-1.0);
}
if (localHitPoint[1].y < decimal(-mHalfHeight)) {
tHit[1] = decimal(-1.0);
}
// If the ray is in direction of the base plane of the cone
if (r.y > epsilon) {
// Compute the intersection with the base plane of the cone
tHit[2] = (-ray.point1.y - mHalfHeight) / (r.y);
// Only keep this intersection if it is inside the cone radius
localHitPoint[2] = ray.point1 + tHit[2] * r;
if ((localHitPoint[2] - centerBase).lengthSquare() > mRadius * mRadius) {
tHit[2] = decimal(-1.0);
}
// Compute the normal direction
localNormal[2] = axis;
}
// Find the smallest positive t value
int hitIndex = -1;
decimal t = DECIMAL_LARGEST;
for (int i=0; i<3; i++) {
if (tHit[i] < decimal(0.0)) continue;
if (tHit[i] < t) {
hitIndex = i;
t = tHit[hitIndex];
}
}
if (hitIndex < 0) return false;
if (t > ray.maxFraction) return false;
// Compute the normal direction for hit against side of the cone
if (hitIndex != 2) {
decimal h = decimal(2.0) * mHalfHeight;
decimal value1 = (localHitPoint[hitIndex].x * localHitPoint[hitIndex].x +
localHitPoint[hitIndex].z * localHitPoint[hitIndex].z);
decimal rOverH = mRadius / h;
decimal value2 = decimal(1.0) + rOverH * rOverH;
decimal factor = decimal(1.0) / std::sqrt(value1 * value2);
decimal x = localHitPoint[hitIndex].x * factor;
decimal z = localHitPoint[hitIndex].z * factor;
localNormal[hitIndex].x = x;
localNormal[hitIndex].y = std::sqrt(x * x + z * z) * rOverH;
localNormal[hitIndex].z = z;
}
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint[hitIndex];
raycastInfo.worldNormal = localNormal[hitIndex];
return true;
}

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

@ -1,194 +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_CONE_SHAPE_H
#define REACTPHYSICS3D_CONE_SHAPE_H
// Libraries
#include "ConvexShape.h"
#include "body/CollisionBody.h"
#include "mathematics/mathematics.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Class ConeShape
/**
* This class represents a cone collision shape centered at the
* origin and alligned with the Y axis. The cone is defined
* by its height and by the radius of its base. The center of the
* cone is at the half of the height. The "transform" of the
* corresponding rigid body gives an orientation and a position
* to the cone. This collision shape uses an extra margin distance around
* it for collision detection purpose. The default margin is 4cm (if your
* units are meters, which is recommended). In case, you want to simulate small
* objects (smaller than the margin distance), you might want to reduce the margin
* by specifying your own margin distance using the "margin" parameter in the
* constructor of the cone shape. Otherwise, it is recommended to use the
* default margin distance by not using the "margin" parameter in the constructor.
*/
class ConeShape : public ConvexShape {
protected :
// -------------------- Attributes -------------------- //
/// Radius of the base
decimal mRadius;
/// Half height of the cone
decimal mHalfHeight;
/// sine of the semi angle at the apex point
decimal mSinTheta;
// -------------------- Methods -------------------- //
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const override;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
public :
// -------------------- Methods -------------------- //
/// Constructor
ConeShape(decimal mRadius, decimal height, decimal margin = OBJECT_MARGIN);
/// Destructor
virtual ~ConeShape() override = default;
/// Deleted copy-constructor
ConeShape(const ConeShape& shape) = delete;
/// Deleted assignment operator
ConeShape& operator=(const ConeShape& shape) = delete;
/// Return the radius
decimal getRadius() const;
/// Return the height
decimal getHeight() const;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling) override;
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const override;
/// 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 override;
};
// Return the radius
/**
* @return Radius of the cone (in meters)
*/
inline decimal ConeShape::getRadius() const {
return mRadius;
}
// Return the height
/**
* @return Height of the cone (in meters)
*/
inline decimal ConeShape::getHeight() const {
return decimal(2.0) * mHalfHeight;
}
// Set the scaling vector of the collision shape
inline void ConeShape::setLocalScaling(const Vector3& scaling) {
mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y;
mRadius = (mRadius / mScaling.x) * scaling.x;
CollisionShape::setLocalScaling(scaling);
}
// Return the number of bytes used by the collision shape
inline size_t ConeShape::getSizeInBytes() const {
return sizeof(ConeShape);
}
// 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
* @param max The maximum bounds of the shape in local-space coordinates
*/
inline void ConeShape::getLocalBounds(Vector3& min, Vector3& max) const {
// Maximum bounds
max.x = mRadius + mMargin;
max.y = mHalfHeight + mMargin;
max.z = max.x;
// Minimum bounds
min.x = -max.x;
min.y = -max.y;
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
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
decimal rSquare = mRadius * mRadius;
decimal diagXZ = decimal(0.15) * mass * (rSquare + mHalfHeight);
tensor.setAllValues(diagXZ, 0.0, 0.0,
0.0, decimal(0.3) * mass * rSquare,
0.0, 0.0, 0.0, diagXZ);
}
// Return true if a point is inside the collision shape
inline bool ConeShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
const decimal radiusHeight = mRadius * (-localPoint.y + mHalfHeight) /
(mHalfHeight * decimal(2.0));
return (localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight) &&
(localPoint.x * localPoint.x + localPoint.z * localPoint.z < radiusHeight *radiusHeight);
}
}
#endif

203
src/collision/shapes/ConvexMeshShape.cpp
View File

@ -30,6 +30,9 @@
using namespace reactphysics3d;
// TODO : Check in every collision shape that localScalling is used correctly and even with SAT
// algorithm (not only in getLocalSupportPoint***() methods)
// Constructor to initialize with an array of 3D vertices.
/// This method creates an internal copy of the input vertices.
/**
@ -38,108 +41,13 @@ using namespace reactphysics3d;
* @param stride Stride between the beginning of two elements in the vertices array
* @param margin Collision margin (in meters) around the collision shape
*/
ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride, decimal margin)
: ConvexShape(CollisionShapeType::CONVEX_MESH, margin), mNbVertices(nbVertices), mMinBounds(0, 0, 0),
mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) {
assert(nbVertices > 0);
assert(stride > 0);
const unsigned char* vertexPointer = (const unsigned char*) arrayVertices;
// Copy all the vertices into the internal array
for (uint i=0; i<mNbVertices; i++) {
const decimal* newPoint = (const decimal*) vertexPointer;
mVertices.push_back(Vector3(newPoint[0], newPoint[1], newPoint[2]));
vertexPointer += stride;
}
ConvexMeshShape::ConvexMeshShape(PolyhedronMesh* polyhedronMesh)
: ConvexPolyhedronShape(CollisionShapeName::CONVEX_MESH), mPolyhedronMesh(polyhedronMesh), mMinBounds(0, 0, 0), mMaxBounds(0, 0, 0) {
// Recalculate the bounds of the mesh
recalculateBounds();
}
// Constructor to initialize with a triangle mesh
/// This method creates an internal copy of the input vertices.
/**
* @param triangleVertexArray Array with the vertices and indices of the vertices and triangles of the mesh
* @param isEdgesInformationUsed True if you want to use edges information for collision detection (faster but requires more memory)
* @param margin Collision margin (in meters) around the collision shape
*/
ConvexMeshShape::ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed, decimal margin)
: ConvexShape(CollisionShapeType::CONVEX_MESH, margin), mMinBounds(0, 0, 0),
mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(isEdgesInformationUsed) {
TriangleVertexArray::VertexDataType vertexType = triangleVertexArray->getVertexDataType();
TriangleVertexArray::IndexDataType indexType = triangleVertexArray->getIndexDataType();
unsigned char* verticesStart = triangleVertexArray->getVerticesStart();
unsigned char* indicesStart = triangleVertexArray->getIndicesStart();
int vertexStride = triangleVertexArray->getVerticesStride();
int indexStride = triangleVertexArray->getIndicesStride();
// For each vertex of the mesh
for (uint v = 0; v < triangleVertexArray->getNbVertices(); v++) {
// Get the vertices components of the triangle
if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_FLOAT_TYPE) {
const float* vertices = (float*)(verticesStart + v * vertexStride);
Vector3 vertex(vertices[0], vertices[1], vertices[2] );
vertex = vertex * mScaling;
mVertices.push_back(vertex);
}
else if (vertexType == TriangleVertexArray::VertexDataType::VERTEX_DOUBLE_TYPE) {
const double* vertices = (double*)(verticesStart + v * vertexStride);
Vector3 vertex(vertices[0], vertices[1], vertices[2] );
vertex = vertex * mScaling;
mVertices.push_back(vertex);
}
}
// If we need to use the edges information of the mesh
if (mIsEdgesInformationUsed) {
// For each triangle of the mesh
for (uint triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) {
void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
uint vertexIndex[3] = {0, 0, 0};
// For each vertex of the triangle
for (int k=0; k < 3; k++) {
// Get the index of the current vertex in the triangle
if (indexType == TriangleVertexArray::IndexDataType::INDEX_INTEGER_TYPE) {
vertexIndex[k] = ((uint*)vertexIndexPointer)[k];
}
else if (indexType == TriangleVertexArray::IndexDataType::INDEX_SHORT_TYPE) {
vertexIndex[k] = ((unsigned short*)vertexIndexPointer)[k];
}
else {
assert(false);
}
}
// Add information about the edges
addEdge(vertexIndex[0], vertexIndex[1]);
addEdge(vertexIndex[0], vertexIndex[2]);
addEdge(vertexIndex[1], vertexIndex[2]);
}
}
mNbVertices = mVertices.size();
recalculateBounds();
}
// Constructor.
/// If you use this constructor, you will need to set the vertices manually one by one using
/// the addVertex() method.
ConvexMeshShape::ConvexMeshShape(decimal margin)
: ConvexShape(CollisionShapeType::CONVEX_MESH, margin), mNbVertices(0), mMinBounds(0, 0, 0),
mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) {
}
// Return a local support point in a given direction without the object margin.
/// If the edges information is not used for collision detection, this method will go through
/// the whole vertices list and pick up the vertex with the largest dot product in the support
@ -148,81 +56,28 @@ ConvexMeshShape::ConvexMeshShape(decimal margin)
/// it as a start in a hill-climbing (local search) process to find the new support vertex which
/// will be in most of the cases very close to the previous one. Using hill-climbing, this method
/// runs in almost constant time.
Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const {
assert(mNbVertices == mVertices.size());
assert(cachedCollisionData != nullptr);
double maxDotProduct = DECIMAL_SMALLEST;
uint indexMaxDotProduct = 0;
// Allocate memory for the cached collision data if not allocated yet
if ((*cachedCollisionData) == nullptr) {
*cachedCollisionData = (int*) malloc(sizeof(int));
*((int*)(*cachedCollisionData)) = 0;
}
// For each vertex of the mesh
for (uint i=0; i<mPolyhedronMesh->getNbVertices(); i++) {
// If the edges information is used to speed up the collision detection
if (mIsEdgesInformationUsed) {
// Compute the dot product of the current vertex
double dotProduct = direction.dot(mPolyhedronMesh->getVertex(i));
assert(mEdgesAdjacencyList.size() == mNbVertices);
uint maxVertex = *((int*)(*cachedCollisionData));
decimal maxDotProduct = direction.dot(mVertices[maxVertex]);
bool isOptimal;
// Perform hill-climbing (local search)
do {
isOptimal = true;
assert(mEdgesAdjacencyList.at(maxVertex).size() > 0);
// For all neighbors of the current vertex
std::set<uint>::const_iterator it;
std::set<uint>::const_iterator itBegin = mEdgesAdjacencyList.at(maxVertex).begin();
std::set<uint>::const_iterator itEnd = mEdgesAdjacencyList.at(maxVertex).end();
for (it = itBegin; it != itEnd; ++it) {
// Compute the dot product
decimal dotProduct = direction.dot(mVertices[*it]);
// If the current vertex is a better vertex (larger dot product)
if (dotProduct > maxDotProduct) {
maxVertex = *it;
maxDotProduct = dotProduct;
isOptimal = false;
}
}
} while(!isOptimal);
// Cache the support vertex
*((int*)(*cachedCollisionData)) = maxVertex;
// Return the support vertex
return mVertices[maxVertex] * mScaling;
}
else { // If the edges information is not used
double maxDotProduct = DECIMAL_SMALLEST;
uint indexMaxDotProduct = 0;
// For each vertex of the mesh
for (uint i=0; i<mNbVertices; i++) {
// Compute the dot product of the current vertex
double dotProduct = direction.dot(mVertices[i]);
// If the current dot product is larger than the maximum one
if (dotProduct > maxDotProduct) {
indexMaxDotProduct = i;
maxDotProduct = dotProduct;
}
// If the current dot product is larger than the maximum one
if (dotProduct > maxDotProduct) {
indexMaxDotProduct = i;
maxDotProduct = dotProduct;
}
assert(maxDotProduct >= decimal(0.0));
// Return the vertex with the largest dot product in the support direction
return mVertices[indexMaxDotProduct] * mScaling;
}
assert(maxDotProduct >= decimal(0.0));
// Return the vertex with the largest dot product in the support direction
return mPolyhedronMesh->getVertex(indexMaxDotProduct) * mScaling;
}
// Recompute the bounds of the mesh
@ -235,16 +90,16 @@ void ConvexMeshShape::recalculateBounds() {
mMaxBounds.setToZero();
// For each vertex of the mesh
for (uint i=0; i<mNbVertices; i++) {
for (uint i=0; i<mPolyhedronMesh->getNbVertices(); i++) {
if (mVertices[i].x > mMaxBounds.x) mMaxBounds.x = mVertices[i].x;
if (mVertices[i].x < mMinBounds.x) mMinBounds.x = mVertices[i].x;
if (mPolyhedronMesh->getVertex(i).x > mMaxBounds.x) mMaxBounds.x = mPolyhedronMesh->getVertex(i).x;
if (mPolyhedronMesh->getVertex(i).x < mMinBounds.x) mMinBounds.x = mPolyhedronMesh->getVertex(i).x;
if (mVertices[i].y > mMaxBounds.y) mMaxBounds.y = mVertices[i].y;
if (mVertices[i].y < mMinBounds.y) mMinBounds.y = mVertices[i].y;
if (mPolyhedronMesh->getVertex(i).y > mMaxBounds.y) mMaxBounds.y = mPolyhedronMesh->getVertex(i).y;
if (mPolyhedronMesh->getVertex(i).y < mMinBounds.y) mMinBounds.y = mPolyhedronMesh->getVertex(i).y;
if (mVertices[i].z > mMaxBounds.z) mMaxBounds.z = mVertices[i].z;
if (mVertices[i].z < mMinBounds.z) mMinBounds.z = mVertices[i].z;
if (mPolyhedronMesh->getVertex(i).z > mMaxBounds.z) mMaxBounds.z = mPolyhedronMesh->getVertex(i).z;
if (mPolyhedronMesh->getVertex(i).z < mMinBounds.z) mMinBounds.z = mPolyhedronMesh->getVertex(i).z;
}
// Apply the local scaling factor
@ -257,7 +112,7 @@ void ConvexMeshShape::recalculateBounds() {
}
// Raycast method with feedback information
bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const {
return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast(
ray, proxyShape, raycastInfo);
}

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

@ -1,4 +1,4 @@
/********************************************************************************
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2016 Daniel Chappuis *
*********************************************************************************
@ -27,10 +27,11 @@
#define REACTPHYSICS3D_CONVEX_MESH_SHAPE_H
// Libraries
#include "ConvexShape.h"
#include "ConvexPolyhedronShape.h"
#include "engine/CollisionWorld.h"
#include "mathematics/mathematics.h"
#include "collision/TriangleMesh.h"
#include "collision/PolyhedronMesh.h"
#include "collision/narrowphase/GJK/GJKAlgorithm.h"
#include <vector>
#include <set>
@ -45,30 +46,18 @@ class CollisionWorld;
// Class ConvexMeshShape
/**
* This class represents a convex mesh shape. In order to create a convex mesh shape, you
* need to indicate the local-space position of the mesh vertices. You do it either by
* passing a vertices array to the constructor or using the addVertex() method. Make sure
* that the set of vertices that you use to create the shape are indeed part of a convex
* mesh. The center of mass of the shape will be at the origin of the local-space geometry
* that you use to create the mesh. The method used for collision detection with a convex
* mesh shape has an O(n) running time with "n" beeing the number of vertices in the mesh.
* Therefore, you should try not to use too many vertices. However, it is possible to speed
* up the collision detection by using the edges information of your mesh. The running time
* of the collision detection that uses the edges is almost O(1) constant time at the cost
* of additional memory used to store the vertices. You can indicate edges information
* with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method
* in order to use the edges information for collision detection.
* need to indicate the local-space position of the mesh vertices. The center of mass
* of the shape will be at the origin of the local-space geometry that you use to create
* the mesh.
*/
class ConvexMeshShape : public ConvexShape {
class ConvexMeshShape : public ConvexPolyhedronShape {
protected :
// -------------------- Attributes -------------------- //
/// Array with the vertices of the mesh
std::vector<Vector3> mVertices;
/// Number of vertices in the mesh
uint mNbVertices;
/// Polyhedron structure of the mesh
PolyhedronMesh* mPolyhedronMesh;
/// Mesh minimum bounds in the three local x, y and z directions
Vector3 mMinBounds;
@ -76,30 +65,19 @@ class ConvexMeshShape : public ConvexShape {
/// Mesh maximum bounds in the three local x, y and z directions
Vector3 mMaxBounds;
/// True if the shape contains the edges of the convex mesh in order to
/// make the collision detection faster
bool mIsEdgesInformationUsed;
/// Adjacency list representing the edges of the mesh
std::map<uint, std::set<uint> > mEdgesAdjacencyList;
// -------------------- Methods -------------------- //
/// Recompute the bounds of the mesh
void recalculateBounds();
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling) override;
/// Return a local support point in a given direction without the object margin.
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const override;
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
@ -108,16 +86,8 @@ class ConvexMeshShape : public ConvexShape {
// -------------------- Methods -------------------- //
/// Constructor to initialize with an array of 3D vertices.
ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride,
decimal margin = OBJECT_MARGIN);
/// Constructor to initialize with a triangle vertex array
ConvexMeshShape(TriangleVertexArray* triangleVertexArray, bool isEdgesInformationUsed = true,
decimal margin = OBJECT_MARGIN);
/// Constructor.
ConvexMeshShape(decimal margin = OBJECT_MARGIN);
/// Constructor
ConvexMeshShape(PolyhedronMesh* polyhedronMesh);
/// Destructor
virtual ~ConvexMeshShape() override = default;
@ -128,27 +98,41 @@ class ConvexMeshShape : public ConvexShape {
/// Deleted assignment operator
ConvexMeshShape& operator=(const ConvexMeshShape& shape) = delete;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling) override;
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const override;
/// Return the local inertia tensor of the collision shape.
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override;
/// Add a vertex into the convex mesh
void addVertex(const Vector3& vertex);
/// Return the number of faces of the polyhedron
virtual uint getNbFaces() const override;
/// Add an edge into the convex mesh by specifying the two vertex indices of the edge.
void addEdge(uint v1, uint v2);
/// Return a given face of the polyhedron
virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const override;
/// Return true if the collision shape is a polyhedron
virtual bool isPolyhedron() const;
/// Return the number of vertices of the polyhedron
virtual uint getNbVertices() const override;
/// Return true if the edges information is used to speed up the collision detection
bool isEdgesInformationUsed() const;
/// Return a given vertex of the polyhedron
virtual HalfEdgeStructure::Vertex getVertex(uint vertexIndex) const override;
/// Set the variable to know if the edges information is used to speed up the
/// collision detection
void setIsEdgesInformationUsed(bool isEdgesUsed);
/// Return the number of half-edges of the polyhedron
virtual uint getNbHalfEdges() const override;
/// Return a given half-edge of the polyhedron
virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const override;
/// Return the position of a given vertex
virtual Vector3 getVertexPosition(uint vertexIndex) const override;
/// Return the normal vector of a given face of the polyhedron
virtual Vector3 getFaceNormal(uint faceIndex) const override;
/// Return the centroid of the polyhedron
virtual Vector3 getCentroid() const override;
};
/// Set the scaling vector of the collision shape
@ -162,11 +146,6 @@ 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
@ -197,68 +176,6 @@ inline void ConvexMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decima
0.0, 0.0, factor * (xSquare + ySquare));
}
// Add a vertex into the convex mesh
/**
* @param vertex Vertex to be added
*/
inline void ConvexMeshShape::addVertex(const Vector3& vertex) {
// Add the vertex in to vertices array
mVertices.push_back(vertex);
mNbVertices++;
// Update the bounds of the mesh
if (vertex.x * mScaling.x > mMaxBounds.x) mMaxBounds.x = vertex.x * mScaling.x;
if (vertex.x * mScaling.x < mMinBounds.x) mMinBounds.x = vertex.x * mScaling.x;
if (vertex.y * mScaling.y > mMaxBounds.y) mMaxBounds.y = vertex.y * mScaling.y;
if (vertex.y * mScaling.y < mMinBounds.y) mMinBounds.y = vertex.y * mScaling.y;
if (vertex.z * mScaling.z > mMaxBounds.z) mMaxBounds.z = vertex.z * mScaling.z;
if (vertex.z * mScaling.z < mMinBounds.z) mMinBounds.z = vertex.z * mScaling.z;
}
// Add an edge into the convex mesh by specifying the two vertex indices of the edge.
/// Note that the vertex indices start at zero and need to correspond to the order of
/// the vertices in the vertices array in the constructor or the order of the calls
/// of the addVertex() methods that you use to add vertices into the convex mesh.
/**
* @param v1 Index of the first vertex of the edge to add
* @param v2 Index of the second vertex of the edge to add
*/
inline void ConvexMeshShape::addEdge(uint v1, uint v2) {
// If the entry for vertex v1 does not exist in the adjacency list
if (mEdgesAdjacencyList.count(v1) == 0) {
mEdgesAdjacencyList.insert(std::make_pair(v1, std::set<uint>()));
}
// If the entry for vertex v2 does not exist in the adjacency list
if (mEdgesAdjacencyList.count(v2) == 0) {
mEdgesAdjacencyList.insert(std::make_pair(v2, std::set<uint>()));
}
// Add the edge in the adjacency list
mEdgesAdjacencyList[v1].insert(v2);
mEdgesAdjacencyList[v2].insert(v1);
}
// Return true if the edges information is used to speed up the collision detection
/**
* @return True if the edges information is used and false otherwise
*/
inline bool ConvexMeshShape::isEdgesInformationUsed() const {
return mIsEdgesInformationUsed;
}
// Set the variable to know if the edges information is used to speed up the
// collision detection
/**
* @param isEdgesUsed True if you want to use the edges information to speed up
* the collision detection with the convex mesh shape
*/
inline void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) {
mIsEdgesInformationUsed = isEdgesUsed;
}
// Return true if a point is inside the collision shape
inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint,
ProxyShape* proxyShape) const {
@ -268,6 +185,56 @@ inline bool ConvexMeshShape::testPointInside(const Vector3& localPoint,
mNarrowPhaseGJKAlgorithm.testPointInside(localPoint, proxyShape);
}
// Return the number of faces of the polyhedron
inline uint ConvexMeshShape::getNbFaces() const {
return mPolyhedronMesh->getHalfEdgeStructure().getNbFaces();
}
// Return a given face of the polyhedron
inline const HalfEdgeStructure::Face& ConvexMeshShape::getFace(uint faceIndex) const {
assert(faceIndex < getNbFaces());
return mPolyhedronMesh->getHalfEdgeStructure().getFace(faceIndex);
}
// Return the number of vertices of the polyhedron
inline uint ConvexMeshShape::getNbVertices() const {
return mPolyhedronMesh->getHalfEdgeStructure().getNbVertices();
}
// Return a given vertex of the polyhedron
inline HalfEdgeStructure::Vertex ConvexMeshShape::getVertex(uint vertexIndex) const {
assert(vertexIndex < getNbVertices());
return mPolyhedronMesh->getHalfEdgeStructure().getVertex(vertexIndex);
}
// Return the number of half-edges of the polyhedron
inline uint ConvexMeshShape::getNbHalfEdges() const {
return mPolyhedronMesh->getHalfEdgeStructure().getNbHalfEdges();
}
// Return a given half-edge of the polyhedron
inline const HalfEdgeStructure::Edge& ConvexMeshShape::getHalfEdge(uint edgeIndex) const {
assert(edgeIndex < getNbHalfEdges());
return mPolyhedronMesh->getHalfEdgeStructure().getHalfEdge(edgeIndex);
}
// Return the position of a given vertex
inline Vector3 ConvexMeshShape::getVertexPosition(uint vertexIndex) const {
assert(vertexIndex < getNbVertices());
return mPolyhedronMesh->getVertex(vertexIndex) * mScaling;
}
// Return the normal vector of a given face of the polyhedron
inline Vector3 ConvexMeshShape::getFaceNormal(uint faceIndex) const {
assert(faceIndex < getNbFaces());
return mPolyhedronMesh->getFaceNormal(faceIndex);
}
// Return the centroid of the polyhedron
inline Vector3 ConvexMeshShape::getCentroid() const {
return mPolyhedronMesh->getCentroid() * mScaling;
}
}
#endif

59
src/collision/shapes/ConvexPolyhedronShape.cpp Normal file
View File

@ -0,0 +1,59 @@
/********************************************************************************
* 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 "ConvexPolyhedronShape.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
ConvexPolyhedronShape::ConvexPolyhedronShape(CollisionShapeName name)
: ConvexShape(name, CollisionShapeType::CONVEX_POLYHEDRON) {
}
// Find and return the index of the polyhedron face with the most anti-parallel face
// normal given a direction vector. This is used to find the incident face on
// a polyhedron of a given reference face of another polyhedron
uint ConvexPolyhedronShape::findMostAntiParallelFace(const Vector3& direction) const {
decimal minDotProduct = DECIMAL_LARGEST;
uint mostAntiParallelFace = 0;
// For each face of the polyhedron
for (uint i=0; i < getNbFaces(); i++) {
// Get the face normal
decimal dotProduct = getFaceNormal(i).dot(direction);
if (dotProduct < minDotProduct) {
minDotProduct = dotProduct;
mostAntiParallelFace = i;
}
}
return mostAntiParallelFace;
}

105
src/collision/shapes/ConvexPolyhedronShape.h Normal file
View File

@ -0,0 +1,105 @@
/********************************************************************************
* 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_CONVEX_POLYHEDRON_H
#define REACTPHYSICS3D_CONVEX_POLYHEDRON_H
// Libraries
#include "ConvexShape.h"
#include "collision/HalfEdgeStructure.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Class ConvexPolyhedronShape
/**
* This abstract class represents a convex polyhedron collision shape associated with a
* body that is used during the narrow-phase collision detection.
*/
class ConvexPolyhedronShape : public ConvexShape {
protected :
public :
// -------------------- Methods -------------------- //
/// Constructor
ConvexPolyhedronShape(CollisionShapeName name);
/// Destructor
virtual ~ConvexPolyhedronShape() override = default;
/// Deleted copy-constructor
ConvexPolyhedronShape(const ConvexPolyhedronShape& shape) = delete;
/// Deleted assignment operator
ConvexPolyhedronShape& operator=(const ConvexPolyhedronShape& shape) = delete;
/// Return the number of faces of the polyhedron
virtual uint getNbFaces() const=0;
/// Return a given face of the polyhedron
virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const=0;
/// Return the number of vertices of the polyhedron
virtual uint getNbVertices() const=0;
/// Return a given vertex of the polyhedron
virtual HalfEdgeStructure::Vertex getVertex(uint vertexIndex) const=0;
/// Return the position of a given vertex
virtual Vector3 getVertexPosition(uint vertexIndex) const=0;
/// Return the normal vector of a given face of the polyhedron
virtual Vector3 getFaceNormal(uint faceIndex) const=0;
/// Return the number of half-edges of the polyhedron
virtual uint getNbHalfEdges() const=0;
/// Return a given half-edge of the polyhedron
virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const=0;
/// Return true if the collision shape is a polyhedron
virtual bool isPolyhedron() const override;
/// Return the centroid of the polyhedron
virtual Vector3 getCentroid() const=0;
/// Find and return the index of the polyhedron face with the most anti-parallel face
/// normal given a direction vector
uint findMostAntiParallelFace(const Vector3& direction) const;
};
// Return true if the collision shape is a polyhedron
inline bool ConvexPolyhedronShape::isPolyhedron() const {
return true;
}
}
#endif

9
src/collision/shapes/ConvexShape.cpp
View File

@ -31,17 +31,16 @@
using namespace reactphysics3d;
// Constructor
ConvexShape::ConvexShape(CollisionShapeType type, decimal margin)
: CollisionShape(type), mMargin(margin) {
ConvexShape::ConvexShape(CollisionShapeName name, CollisionShapeType type, decimal margin)
: CollisionShape(name, type), mMargin(margin) {
}
// Return a local support point in a given direction with the object margin
Vector3 ConvexShape::getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const {
Vector3 ConvexShape::getLocalSupportPointWithMargin(const Vector3& direction) const {
// Get the support point without margin
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction);
if (mMargin != decimal(0.0)) {

12
src/collision/shapes/ConvexShape.h
View File

@ -49,19 +49,17 @@ class ConvexShape : public CollisionShape {
// -------------------- Methods -------------------- //
// Return a local support point in a given direction with the object margin
Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const;
Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const;
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const=0;
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const=0;
public :
// -------------------- Methods -------------------- //
/// Constructor
ConvexShape(CollisionShapeType type, decimal margin);
ConvexShape(CollisionShapeName name, CollisionShapeType type, decimal margin = decimal(0.0));
/// Destructor
virtual ~ConvexShape() override = default;
@ -81,10 +79,10 @@ class ConvexShape : public CollisionShape {
// -------------------- Friendship -------------------- //
friend class GJKAlgorithm;
friend class EPAAlgorithm;
friend class SATAlgorithm;
};
/// 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 ConvexShape::isConvex() const {
return true;
}

231
src/collision/shapes/CylinderShape.cpp
View File

@ -1,231 +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 "CylinderShape.h"
#include "collision/ProxyShape.h"
#include "configuration.h"
using namespace reactphysics3d;
// Constructor
/**
* @param radius Radius of the cylinder (in meters)
* @param height Height of the cylinder (in meters)
* @param margin Collision margin (in meters) around the collision shape
*/
CylinderShape::CylinderShape(decimal radius, decimal height, decimal margin)
: ConvexShape(CollisionShapeType::CYLINDER, margin), mRadius(radius),
mHalfHeight(height/decimal(2.0)) {
assert(radius > decimal(0.0));
assert(height > decimal(0.0));
}
// Return a local support point in a given direction without the object margin
Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
Vector3 supportPoint(0.0, 0.0, 0.0);
decimal uDotv = direction.y;
Vector3 w(direction.x, 0.0, direction.z);
decimal lengthW = std::sqrt(direction.x * direction.x + direction.z * direction.z);
if (lengthW > MACHINE_EPSILON) {
if (uDotv < 0.0) supportPoint.y = -mHalfHeight;
else supportPoint.y = mHalfHeight;
supportPoint += (mRadius / lengthW) * w;
}
else {
if (uDotv < 0.0) supportPoint.y = -mHalfHeight;
else supportPoint.y = mHalfHeight;
}
return supportPoint;
}
// Raycast method with feedback information
/// Algorithm based on the one described at page 194 in Real-ime Collision Detection by
/// Morgan Kaufmann.
bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
const Vector3 n = ray.point2 - ray.point1;
const decimal epsilon = decimal(0.01);
Vector3 p(decimal(0), -mHalfHeight, decimal(0));
Vector3 q(decimal(0), mHalfHeight, decimal(0));
Vector3 d = q - p;
Vector3 m = ray.point1 - p;
decimal t;
decimal mDotD = m.dot(d);
decimal nDotD = n.dot(d);
decimal dDotD = d.dot(d);
// Test if the segment is outside the cylinder
if (mDotD < decimal(0.0) && mDotD + nDotD < decimal(0.0)) return false;
if (mDotD > dDotD && mDotD + nDotD > dDotD) return false;
decimal nDotN = n.dot(n);
decimal mDotN = m.dot(n);
decimal a = dDotD * nDotN - nDotD * nDotD;
decimal k = m.dot(m) - mRadius * mRadius;
decimal c = dDotD * k - mDotD * mDotD;
// If the ray is parallel to the cylinder axis
if (std::abs(a) < epsilon) {
// If the origin is outside the surface of the cylinder, we return no hit
if (c > decimal(0.0)) return false;
// Here we know that the segment intersect an endcap of the cylinder
// If the ray intersects with the "p" endcap of the cylinder
if (mDotD < decimal(0.0)) {
t = -mDotN / nDotN;
// If the intersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(-1), 0);
raycastInfo.worldNormal = normalDirection;
return true;
}
else if (mDotD > dDotD) { // If the ray intersects with the "q" endcap of the cylinder
t = (nDotD - mDotN) / nDotN;
// If the intersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(1.0), 0);
raycastInfo.worldNormal = normalDirection;
return true;
}
else { // If the origin is inside the cylinder, we return no hit
return false;
}
}
decimal b = dDotD * mDotN - nDotD * mDotD;
decimal discriminant = b * b - a * c;
// If the discriminant is negative, no real roots and therfore, no hit
if (discriminant < decimal(0.0)) return false;
// Compute the smallest root (first intersection along the ray)
decimal t0 = t = (-b - std::sqrt(discriminant)) / a;
// If the intersection is outside the cylinder on "p" endcap side
decimal value = mDotD + t * nDotD;
if (value < decimal(0.0)) {
// If the ray is pointing away from the "p" endcap, we return no hit
if (nDotD <= decimal(0.0)) return false;
// Compute the intersection against the "p" endcap (intersection agains whole plane)
t = -mDotD / nDotD;
// Keep the intersection if the it is inside the cylinder radius
if (k + t * (decimal(2.0) * mDotN + t) > decimal(0.0)) return false;
// If the intersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(-1.0), 0);
raycastInfo.worldNormal = normalDirection;
return true;
}
else if (value > dDotD) { // If the intersection is outside the cylinder on the "q" side
// If the ray is pointing away from the "q" endcap, we return no hit
if (nDotD >= decimal(0.0)) return false;
// Compute the intersection against the "q" endcap (intersection against whole plane)
t = (dDotD - mDotD) / nDotD;
// Keep the intersection if it is inside the cylinder radius
if (k + dDotD - decimal(2.0) * mDotD + t * (decimal(2.0) * (mDotN - nDotD) + t) >
decimal(0.0)) return false;
// If the intersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(1.0), 0);
raycastInfo.worldNormal = normalDirection;
return true;
}
t = t0;
// If the intersection is behind the origin of the ray or beyond the maximum
// raycasting distance, we return no hit
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localHitPoint;
Vector3 v = localHitPoint - p;
Vector3 w = (v.dot(d) / d.lengthSquare()) * d;
Vector3 normalDirection = (localHitPoint - (p + w));
raycastInfo.worldNormal = normalDirection;
return true;
}

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

@ -1,190 +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_CYLINDER_SHAPE_H
#define REACTPHYSICS3D_CYLINDER_SHAPE_H
// Libraries
#include "ConvexShape.h"
#include "body/CollisionBody.h"
#include "mathematics/mathematics.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Class CylinderShape
/**
* This class represents a cylinder collision shape around the Y axis
* and centered at the origin. The cylinder is defined by its height
* and the radius of its base. The "transform" of the corresponding
* rigid body gives an orientation and a position to the cylinder.
* This collision shape uses an extra margin distance around it for collision
* detection purpose. The default margin is 4cm (if your units are meters,
* which is recommended). In case, you want to simulate small objects
* (smaller than the margin distance), you might want to reduce the margin by
* specifying your own margin distance using the "margin" parameter in the
* constructor of the cylinder shape. Otherwise, it is recommended to use the
* default margin distance by not using the "margin" parameter in the constructor.
*/
class CylinderShape : public ConvexShape {
protected :
// -------------------- Attributes -------------------- //
/// Radius of the base
decimal mRadius;
/// Half height of the cylinder
decimal mHalfHeight;
// -------------------- Methods -------------------- //
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const override;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override ;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
public :
// -------------------- Methods -------------------- //
/// Constructor
CylinderShape(decimal radius, decimal height, decimal margin = OBJECT_MARGIN);
/// Destructor
virtual ~CylinderShape() override = default;
/// Deleted copy-constructor
CylinderShape(const CylinderShape& shape) = delete;
/// Deleted assignment operator
CylinderShape& operator=(const CylinderShape& shape) = delete;
/// Return the radius
decimal getRadius() const;
/// 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) override;
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const override;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const override;
};
// Return the radius
/**
* @return Radius of the cylinder (in meters)
*/
inline decimal CylinderShape::getRadius() const {
return mRadius;
}
// Return the height
/**
* @return Height of the cylinder (in meters)
*/
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) {
mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y;
mRadius = (mRadius / mScaling.x) * scaling.x;
CollisionShape::setLocalScaling(scaling);
}
// Return the number of bytes used by the collision shape
inline size_t CylinderShape::getSizeInBytes() const {
return sizeof(CylinderShape);
}
// 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
* @param max The maximum bounds of the shape in local-space coordinates
*/
inline void CylinderShape::getLocalBounds(Vector3& min, Vector3& max) const {
// Maximum bounds
max.x = mRadius + mMargin;
max.y = mHalfHeight + mMargin;
max.z = max.x;
// Minimum bounds
min.x = -max.x;
min.y = -max.y;
min.z = min.x;
}
// Return the local inertia tensor of the cylinder
/**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
* coordinates
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
decimal height = decimal(2.0) * mHalfHeight;
decimal diag = (decimal(1.0) / decimal(12.0)) * mass * (3 * mRadius * mRadius + height * height);
tensor.setAllValues(diag, 0.0, 0.0, 0.0,
decimal(0.5) * mass * mRadius * mRadius, 0.0,
0.0, 0.0, diag);
}
// Return true if a point is inside the collision shape
inline bool CylinderShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const{
return ((localPoint.x * localPoint.x + localPoint.z * localPoint.z) < mRadius * mRadius &&
localPoint.y < mHalfHeight && localPoint.y > -mHalfHeight);
}
}
#endif

65
src/collision/shapes/HeightFieldShape.cpp
View File

@ -42,7 +42,7 @@ using namespace reactphysics3d;
HeightFieldShape::HeightFieldShape(int nbGridColumns, int nbGridRows, decimal minHeight, decimal maxHeight,
const void* heightFieldData, HeightDataType dataType, int upAxis,
decimal integerHeightScale)
: ConcaveShape(CollisionShapeType::HEIGHTFIELD), mNbColumns(nbGridColumns), mNbRows(nbGridRows),
: ConcaveShape(CollisionShapeName::HEIGHTFIELD), mNbColumns(nbGridColumns), mNbRows(nbGridRows),
mWidth(nbGridColumns - 1), mLength(nbGridRows - 1), mMinHeight(minHeight),
mMaxHeight(maxHeight), mUpAxis(upAxis), mIntegerHeightScale(integerHeightScale),
mHeightDataType(dataType) {
@ -133,24 +133,48 @@ void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB&
for (int j = jMin; j < jMax; j++) {
// Compute the four point of the current quad
Vector3 p1 = getVertexAt(i, j);
Vector3 p2 = getVertexAt(i, j + 1);
Vector3 p3 = getVertexAt(i + 1, j);
Vector3 p4 = getVertexAt(i + 1, j + 1);
const Vector3 p1 = getVertexAt(i, j);
const Vector3 p2 = getVertexAt(i, j + 1);
const Vector3 p3 = getVertexAt(i + 1, j);
const Vector3 p4 = getVertexAt(i + 1, j + 1);
// Generate the first triangle for the current grid rectangle
Vector3 trianglePoints[3] = {p1, p2, p3};
// Compute the triangle normal
Vector3 triangle1Normal = (p2 - p1).cross(p3 - p1).getUnit();
// Use the triangle face normal as vertices normals (this is an aproximation. The correct
// solution would be to compute all the normals of the neighbor triangles and use their
// weighted average (with incident angle as weight) at the vertices. However, this solution
// seems too expensive (it requires to compute the normal of all neighbor triangles instead
// and compute the angle of incident edges with asin(). Maybe we could also precompute the
// vertices normal at the HeightFieldShape constructor but it will require extra memory to
// store them.
Vector3 verticesNormals1[3] = {triangle1Normal, triangle1Normal, triangle1Normal};
// Test collision against the first triangle
callback.testTriangle(trianglePoints);
callback.testTriangle(trianglePoints, verticesNormals1, computeTriangleShapeId(i, j, 0));
// Generate the second triangle for the current grid rectangle
trianglePoints[0] = p3;
trianglePoints[1] = p2;
trianglePoints[2] = p4;
// Compute the triangle normal
Vector3 triangle2Normal = (p2 - p3).cross(p4 - p3).getUnit();
// Use the triangle face normal as vertices normals (this is an aproximation. The correct
// solution would be to compute all the normals of the neighbor triangles and use their
// weighted average (with incident angle as weight) at the vertices. However, this solution
// seems too expensive (it requires to compute the normal of all neighbor triangles instead
// and compute the angle of incident edges with asin(). Maybe we could also precompute the
// vertices normal at the HeightFieldShape constructor but it will require extra memory to
// store them.
Vector3 verticesNormals2[3] = {triangle2Normal, triangle2Normal, triangle2Normal};
// Test collision against the second triangle
callback.testTriangle(trianglePoints);
callback.testTriangle(trianglePoints, verticesNormals2, computeTriangleShapeId(i, j, 1));
}
}
}
@ -188,14 +212,21 @@ void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoor
// Raycast method with feedback information
/// Note that only the first triangle hit by the ray in the mesh will be returned, even if
/// the ray hits many triangles.
bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
bool HeightFieldShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const {
// TODO : Implement raycasting without using an AABB for the ray
// but using a dynamic AABB tree or octree instead
PROFILE("HeightFieldShape::raycast()");
PROFILE("HeightFieldShape::raycast()", mProfiler);
TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this);
TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this, allocator);
#ifdef IS_PROFILING_ACTIVE
// Set the profiler
triangleCallback.setProfiler(mProfiler);
#endif
// Compute the AABB for the ray
const Vector3 rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
@ -232,16 +263,22 @@ Vector3 HeightFieldShape::getVertexAt(int x, int y) const {
}
// Raycast test between a ray and a triangle of the heightfield
void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints) {
void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) {
// Create a triangle collision shape
decimal margin = mHeightFieldShape.getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
TriangleShape triangleShape(trianglePoints, verticesNormals, shapeId, mAllocator);
triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType());
#ifdef IS_PROFILING_ACTIVE
// Set the profiler to the triangle shape
triangleShape.setProfiler(mProfiler);
#endif
// Ray casting test against the collision shape
RaycastInfo raycastInfo;
bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape);
bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape, mAllocator);
// If the ray hit the collision shape
if (isTriangleHit && raycastInfo.hitFraction <= mSmallestHitFraction) {

35
src/collision/shapes/HeightFieldShape.h
View File

@ -49,14 +49,22 @@ class TriangleOverlapCallback : public TriangleCallback {
bool mIsHit;
decimal mSmallestHitFraction;
const HeightFieldShape& mHeightFieldShape;
MemoryAllocator& mAllocator;
#ifdef IS_PROFILING_ACTIVE
/// Pointer to the profiler
Profiler* mProfiler;
#endif
public:
// Constructor
TriangleOverlapCallback(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo,
const HeightFieldShape& heightFieldShape)
const HeightFieldShape& heightFieldShape, MemoryAllocator& allocator)
: mRay(ray), mProxyShape(proxyShape), mRaycastInfo(raycastInfo),
mHeightFieldShape (heightFieldShape) {
mHeightFieldShape (heightFieldShape), mAllocator(allocator) {
mIsHit = false;
mSmallestHitFraction = mRay.maxFraction;
}
@ -64,7 +72,17 @@ class TriangleOverlapCallback : public TriangleCallback {
bool getIsHit() const {return mIsHit;}
/// Raycast test between a ray and a triangle of the heightfield
virtual void testTriangle(const Vector3* trianglePoints) override;
virtual void testTriangle(const Vector3* trianglePoints, const Vector3* verticesNormals, uint shapeId) override;
#ifdef IS_PROFILING_ACTIVE
/// Set the profiler
void setProfiler(Profiler* profiler) {
mProfiler = profiler;
}
#endif
};
@ -126,7 +144,7 @@ class HeightFieldShape : public ConcaveShape {
// -------------------- Methods -------------------- //
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
@ -151,6 +169,9 @@ class HeightFieldShape : public ConcaveShape {
/// Compute the min/max grid coords corresponding to the intersection of the AABB of the height field and the AABB to collide
void computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, const AABB& aabbToCollide) const;
/// Compute the shape Id for a given triangle
uint computeTriangleShapeId(uint iIndex, uint jIndex, uint secondTriangleIncrement) const;
public:
/// Constructor
@ -252,6 +273,12 @@ inline void HeightFieldShape::computeLocalInertiaTensor(Matrix3x3& tensor, decim
0, 0, mass);
}
// Compute the shape Id for a given triangle
inline uint HeightFieldShape::computeTriangleShapeId(uint iIndex, uint jIndex, uint secondTriangleIncrement) const {
return (jIndex * (mNbColumns - 1) + iIndex) * 2 + secondTriangleIncrement;
}
}
#endif

5
src/collision/shapes/SphereShape.cpp
View File

@ -35,12 +35,13 @@ using namespace reactphysics3d;
/**
* @param radius Radius of the sphere (in meters)
*/
SphereShape::SphereShape(decimal radius) : ConvexShape(CollisionShapeType::SPHERE, radius) {
SphereShape::SphereShape(decimal radius)
: ConvexShape(CollisionShapeName::SPHERE, CollisionShapeType::SPHERE, radius) {
assert(radius > decimal(0.0));
}
// Raycast method with feedback information
bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const {
const Vector3 m = ray.point1;
decimal c = m.dot(m) - mMargin * mMargin;

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

@ -49,14 +49,13 @@ class SphereShape : public ConvexShape {
// -------------------- Methods -------------------- //
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const override;
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
@ -81,7 +80,7 @@ class SphereShape : public ConvexShape {
decimal getRadius() const;
/// Return true if the collision shape is a polyhedron
virtual bool isPolyhedron() const;
virtual bool isPolyhedron() const override;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling) override;
@ -123,8 +122,7 @@ inline size_t SphereShape::getSizeInBytes() const {
}
// Return a local support point in a given direction without the object margin
inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const {
// Return the center of the sphere (the radius is taken into account in the object margin)
return Vector3(0.0, 0.0, 0.0);

175
src/collision/shapes/TriangleShape.cpp
View File

@ -26,33 +26,194 @@
// Libraries
#include "TriangleShape.h"
#include "collision/ProxyShape.h"
#include "mathematics/mathematics_functions.h"
#include "engine/Profiler.h"
#include "configuration.h"
#include <cassert>
using namespace reactphysics3d;
// Constructor
/**
* Do not use this constructor. It is supposed to be used internally only.
* Use a ConcaveMeshShape instead.
* @param point1 First point of the triangle
* @param point2 Second point of the triangle
* @param point3 Third point of the triangle
* @param verticesNormals The three vertices normals for smooth mesh collision
* @param margin The collision margin (in meters) around the collision shape
*/
TriangleShape::TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3, decimal margin)
: ConvexShape(CollisionShapeType::TRIANGLE, margin) {
mPoints[0] = point1;
mPoints[1] = point2;
mPoints[2] = point3;
TriangleShape::TriangleShape(const Vector3* vertices, const Vector3* verticesNormals, uint shapeId,
MemoryAllocator& allocator)
: ConvexPolyhedronShape(CollisionShapeName::TRIANGLE), mFaces{HalfEdgeStructure::Face(allocator), HalfEdgeStructure::Face(allocator)} {
mPoints[0] = vertices[0];
mPoints[1] = vertices[1];
mPoints[2] = vertices[2];
// Compute the triangle normal
mNormal = (vertices[1] - vertices[0]).cross(vertices[2] - vertices[0]);
mNormal.normalize();
mVerticesNormals[0] = verticesNormals[0];
mVerticesNormals[1] = verticesNormals[1];
mVerticesNormals[2] = verticesNormals[2];
// Faces
for (uint i=0; i<2; i++) {
mFaces[i].faceVertices.reserve(3);
mFaces[i].faceVertices.add(0);
mFaces[i].faceVertices.add(1);
mFaces[i].faceVertices.add(2);
mFaces[i].edgeIndex = i;
}
// Edges
for (uint i=0; i<6; i++) {
switch(i) {
case 0:
mEdges[0].vertexIndex = 0;
mEdges[0].twinEdgeIndex = 1;
mEdges[0].faceIndex = 0;
mEdges[0].nextEdgeIndex = 2;
break;
case 1:
mEdges[1].vertexIndex = 1;
mEdges[1].twinEdgeIndex = 0;
mEdges[1].faceIndex = 1;
mEdges[1].nextEdgeIndex = 5;
break;
case 2:
mEdges[2].vertexIndex = 1;
mEdges[2].twinEdgeIndex = 3;
mEdges[2].faceIndex = 0;
mEdges[2].nextEdgeIndex = 4;
break;
case 3:
mEdges[3].vertexIndex = 2;
mEdges[3].twinEdgeIndex = 2;
mEdges[3].faceIndex = 1;
mEdges[3].nextEdgeIndex = 1;
break;
case 4:
mEdges[4].vertexIndex = 2;
mEdges[4].twinEdgeIndex = 5;
mEdges[4].faceIndex = 0;
mEdges[4].nextEdgeIndex = 0;
break;
case 5:
mEdges[5].vertexIndex = 0;
mEdges[5].twinEdgeIndex = 4;
mEdges[5].faceIndex = 1;
mEdges[5].nextEdgeIndex = 3;
break;
}
}
mRaycastTestType = TriangleRaycastSide::FRONT;
mId = shapeId;
}
// This method compute the smooth mesh contact with a triangle in case one of the two collision
// shapes is a triangle. The idea in this case is to use a smooth vertex normal of the triangle mesh
// at the contact point instead of the triangle normal to avoid the internal edge collision issue.
// This method will return the new smooth world contact
// normal of the triangle and the the local contact point on the other shape.
void TriangleShape::computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2,
Vector3& localContactPointShape1, Vector3& localContactPointShape2,
const Transform& shape1ToWorld, const Transform& shape2ToWorld,
decimal penetrationDepth, Vector3& outSmoothVertexNormal) {
assert(shape1->getName() != CollisionShapeName::TRIANGLE || shape2->getName() != CollisionShapeName::TRIANGLE);
// If one the shape is a triangle
bool isShape1Triangle = shape1->getName() == CollisionShapeName::TRIANGLE;
if (isShape1Triangle || shape2->getName() == CollisionShapeName::TRIANGLE) {
const TriangleShape* triangleShape = isShape1Triangle ? static_cast<const TriangleShape*>(shape1):
static_cast<const TriangleShape*>(shape2);
// Compute the smooth triangle mesh contact normal and recompute the local contact point on the other shape
triangleShape->computeSmoothMeshContact(isShape1Triangle ? localContactPointShape1 : localContactPointShape2,
isShape1Triangle ? shape1ToWorld : shape2ToWorld,
isShape1Triangle ? shape2ToWorld.getInverse() : shape1ToWorld.getInverse(),
penetrationDepth, isShape1Triangle,
isShape1Triangle ? localContactPointShape2 : localContactPointShape1,
outSmoothVertexNormal);
}
}
// This method implements the technique described in Game Physics Pearl book
// by Gino van der Bergen and Dirk Gregorius to get smooth triangle mesh collision. The idea is
// to replace the contact normal of the triangle shape with the precomputed normal of the triangle
// mesh at this point. Then, we need to recompute the contact point on the other shape in order to
// stay aligned with the new contact normal. This method will return the new smooth world contact
// normal of the triangle and the the local contact point on the other shape.
void TriangleShape::computeSmoothMeshContact(Vector3 localContactPointTriangle, const Transform& triangleShapeToWorldTransform,
const Transform& worldToOtherShapeTransform, decimal penetrationDepth, bool isTriangleShape1,
Vector3& outNewLocalContactPointOtherShape, Vector3& outSmoothWorldContactTriangleNormal) const {
// Get the smooth contact normal of the mesh at the contact point on the triangle
Vector3 triangleLocalNormal = computeSmoothLocalContactNormalForTriangle(localContactPointTriangle);
// Convert the local contact normal into world-space
Vector3 triangleWorldNormal = triangleShapeToWorldTransform.getOrientation() * triangleLocalNormal;
// Penetration axis with direction from triangle to other shape
Vector3 triangleToOtherShapePenAxis = isTriangleShape1 ? outSmoothWorldContactTriangleNormal : -outSmoothWorldContactTriangleNormal;
// The triangle normal should be the one in the direction out of the current colliding face of the triangle
if (triangleWorldNormal.dot(triangleToOtherShapePenAxis) < decimal(0.0)) {
triangleWorldNormal = -triangleWorldNormal;
triangleLocalNormal = -triangleLocalNormal;
}
// Compute the final contact normal from shape 1 to shape 2
outSmoothWorldContactTriangleNormal = isTriangleShape1 ? triangleWorldNormal : -triangleWorldNormal;
// Re-align the local contact point on the other shape such that it is aligned along the new contact normal
Vector3 otherShapePointTriangleSpace = localContactPointTriangle - triangleLocalNormal * penetrationDepth;
Vector3 otherShapePoint = worldToOtherShapeTransform * triangleShapeToWorldTransform * otherShapePointTriangleSpace;
outNewLocalContactPointOtherShape.setAllValues(otherShapePoint.x, otherShapePoint.y, otherShapePoint.z);
}
// Get a smooth contact normal for collision for a triangle of the mesh
/// This is used to avoid the internal edges issue that occurs when a shape is colliding with
/// several triangles of a concave mesh. If the shape collide with an edge of the triangle for instance,
/// the computed contact normal from this triangle edge is not necessarily in the direction of the surface
/// normal of the mesh at this point. The idea to solve this problem is to use the real (smooth) surface
/// normal of the mesh at this point as the contact normal. This technique is described in the chapter 5
/// of the Game Physics Pearl book by Gino van der Bergen and Dirk Gregorius. The vertices normals of the
/// mesh are either provided by the user or precomputed if the user did not provide them. Note that we only
/// use the interpolated normal if the contact point is on an edge of the triangle. If the contact is in the
/// middle of the triangle, we return the true triangle normal.
Vector3 TriangleShape::computeSmoothLocalContactNormalForTriangle(const Vector3& localContactPoint) const {
// Compute the barycentric coordinates of the point in the triangle
decimal u, v, w;
computeBarycentricCoordinatesInTriangle(mPoints[0], mPoints[1], mPoints[2], localContactPoint, u, v, w);
// If the contact is in the middle of the triangle face (not on the edges)
if (u > MACHINE_EPSILON && v > MACHINE_EPSILON && w > MACHINE_EPSILON) {
// We return the true triangle face normal (not the interpolated one)
return mNormal;
}
// We compute the contact normal as the barycentric interpolation of the three vertices normals
return (u * mVerticesNormals[0] + v * mVerticesNormals[1] + w * mVerticesNormals[2]).getUnit();
}
// Raycast method with feedback information
/// This method use the line vs triangle raycasting technique described in
/// Real-time Collision Detection by Christer Ericson.
bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
bool TriangleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape, MemoryAllocator& allocator) const {
PROFILE("TriangleShape::raycast()");
PROFILE("TriangleShape::raycast()", mProfiler);
const Vector3 pq = ray.point2 - ray.point1;
const Vector3 pa = mPoints[0] - ray.point1;

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

@ -28,7 +28,7 @@
// Libraries
#include "mathematics/mathematics.h"
#include "ConvexShape.h"
#include "ConvexPolyhedronShape.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
@ -49,42 +49,71 @@ enum class TriangleRaycastSide {
// Class TriangleShape
/**
* This class represents a triangle collision shape that is centered
* at the origin and defined three points.
* at the origin and defined three points. A user cannot instanciate
* an object of this class. This class is for internal use only. Instances
* of this class are created when the user creates an HeightFieldShape and
* a ConcaveMeshShape
*/
class TriangleShape : public ConvexShape {
class TriangleShape : public ConvexPolyhedronShape {
protected:
// -------------------- Attribute -------------------- //
/// Three points of the triangle
Vector3 mPoints[3];
/// Normal of the triangle
Vector3 mNormal;
/// Three vertices normals for smooth collision with triangle mesh
Vector3 mVerticesNormals[3];
/// Raycast test type for the triangle (front, back, front-back)
TriangleRaycastSide mRaycastTestType;
/// Faces information for the two faces of the triangle
HalfEdgeStructure::Face mFaces[2];
/// Edges information for the six edges of the triangle
HalfEdgeStructure::Edge mEdges[6];
// -------------------- Methods -------------------- //
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const override;
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const override;
/// Get a smooth contact normal for collision for a triangle of the mesh
Vector3 computeSmoothLocalContactNormalForTriangle(const Vector3& localContactPoint) const;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const override;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const override;
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape,
MemoryAllocator& allocator) const override;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const override;
/// Generate the id of the shape (used for temporal coherence)
void generateId();
// -------------------- Methods -------------------- //
/// This method implements the technique described in Game Physics Pearl book
void computeSmoothMeshContact(Vector3 localContactPointTriangle, const Transform& triangleShapeToWorldTransform,
const Transform& worldToOtherShapeTransform, decimal penetrationDepth, bool isTriangleShape1,
Vector3& outNewLocalContactPointOtherShape, Vector3& outSmoothWorldContactTriangleNormal) const;
public:
// -------------------- Methods -------------------- //
/// Constructor
TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3,
decimal margin = OBJECT_MARGIN);
TriangleShape(const Vector3* vertices, const Vector3* verticesNormals,
uint shapeId, MemoryAllocator& allocator);
/// Destructor
virtual ~TriangleShape() override = default;
@ -113,16 +142,44 @@ class TriangleShape : public ConvexShape {
// Set the raycast test type (front, back, front-back)
void setRaycastTestType(TriangleRaycastSide testType);
/// Return the coordinates of a given vertex of the triangle
Vector3 getVertex(int index) const;
/// Return the number of faces of the polyhedron
virtual uint getNbFaces() const override;
/// Return true if the collision shape is a polyhedron
virtual bool isPolyhedron() const;
/// Return a given face of the polyhedron
virtual const HalfEdgeStructure::Face& getFace(uint faceIndex) const override;
/// Return the number of vertices of the polyhedron
virtual uint getNbVertices() const override;
/// Return a given vertex of the polyhedron
virtual HalfEdgeStructure::Vertex getVertex(uint vertexIndex) const override;
/// Return the position of a given vertex
virtual Vector3 getVertexPosition(uint vertexIndex) const override;
/// Return the normal vector of a given face of the polyhedron
virtual Vector3 getFaceNormal(uint faceIndex) const override;
/// Return the number of half-edges of the polyhedron
virtual uint getNbHalfEdges() const override;
/// Return a given half-edge of the polyhedron
virtual const HalfEdgeStructure::Edge& getHalfEdge(uint edgeIndex) const override;
/// Return the centroid of the polyhedron
virtual Vector3 getCentroid() const override;
/// This method compute the smooth mesh contact with a triangle in case one of the two collision shapes is a triangle. The idea in this case is to use a smooth vertex normal of the triangle mesh
static void computeSmoothTriangleMeshContact(const CollisionShape* shape1, const CollisionShape* shape2,
Vector3& localContactPointShape1, Vector3& localContactPointShape2,
const Transform& shape1ToWorld, const Transform& shape2ToWorld,
decimal penetrationDepth, Vector3& outSmoothVertexNormal);
// ---------- Friendship ---------- //
friend class ConcaveMeshRaycastCallback;
friend class TriangleOverlapCallback;
friend class MiddlePhaseTriangleCallback;
};
// Return the number of bytes used by the collision shape
@ -131,8 +188,7 @@ inline size_t TriangleShape::getSizeInBytes() const {
}
// Return a local support point in a given direction without the object margin
inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const {
Vector3 dotProducts(direction.dot(mPoints[0]), direction.dot(mPoints[1]), direction.dot(mPoints[2]));
return mPoints[dotProducts.getMaxAxis()];
}
@ -199,6 +255,63 @@ inline bool TriangleShape::testPointInside(const Vector3& localPoint, ProxyShape
return false;
}
// Return the number of faces of the polyhedron
inline uint TriangleShape::getNbFaces() const {
return 2;
}
// Return a given face of the polyhedron
inline const HalfEdgeStructure::Face& TriangleShape::getFace(uint faceIndex) const {
assert(faceIndex < 2);
return mFaces[faceIndex];
}
// Return the number of vertices of the polyhedron
inline uint TriangleShape::getNbVertices() const {
return 3;
}
// Return a given vertex of the polyhedron
inline HalfEdgeStructure::Vertex TriangleShape::getVertex(uint vertexIndex) const {
assert(vertexIndex < 3);
HalfEdgeStructure::Vertex vertex(vertexIndex);
switch (vertexIndex) {
case 0: vertex.edgeIndex = 0; break;
case 1: vertex.edgeIndex = 2; break;
case 2: vertex.edgeIndex = 4; break;
}
return vertex;
}
// Return a given half-edge of the polyhedron
inline const HalfEdgeStructure::Edge& TriangleShape::getHalfEdge(uint edgeIndex) const {
assert(edgeIndex < getNbHalfEdges());
return mEdges[edgeIndex];
}
// Return the position of a given vertex
inline Vector3 TriangleShape::getVertexPosition(uint vertexIndex) const {
assert(vertexIndex < 3);
return mPoints[vertexIndex];
}
// Return the normal vector of a given face of the polyhedron
inline Vector3 TriangleShape::getFaceNormal(uint faceIndex) const {
assert(faceIndex < 2);
return faceIndex == 0 ? mNormal : -mNormal;
}
// Return the centroid of the box
inline Vector3 TriangleShape::getCentroid() const {
return (mPoints[0] + mPoints[1] + mPoints[2]) / decimal(3.0);
}
// Return the number of half-edges of the polyhedron
inline uint TriangleShape::getNbHalfEdges() const {
return 6;
}
// Return the raycast test type (front, back, front-back)
inline TriangleRaycastSide TriangleShape::getRaycastTestType() const {
return mRaycastTestType;
@ -212,20 +325,6 @@ inline void TriangleShape::setRaycastTestType(TriangleRaycastSide testType) {
mRaycastTestType = testType;
}
// Return the coordinates of a given vertex of the triangle
/**
* @param index Index (0 to 2) of a vertex of the triangle
*/
inline Vector3 TriangleShape::getVertex(int index) const {
assert(index >= 0 && index < 3);
return mPoints[index];
}
// Return true if the collision shape is a polyhedron
inline bool TriangleShape::isPolyhedron() const {
return true;
}
}
#endif

28
src/configuration.h
View File

@ -30,6 +30,7 @@
#include <limits>
#include <cfloat>
#include <utility>
#include <cstdint>
#include "decimal.h"
// Windows platform
@ -47,14 +48,18 @@ namespace reactphysics3d {
// ------------------- Type definitions ------------------- //
using uint = unsigned int;
using uchar = unsigned char;
using ushort = unsigned short;
using luint = long unsigned int;
using bodyindex = luint;
using bodyindexpair = std::pair<bodyindex, bodyindex>;
using int16 = signed short;
using int32 = signed int;
using uint16 = unsigned short;
using uint32 = unsigned int;
using int8 = std::int8_t;
using uint8 = std::uint8_t;
using int16 = std::int16_t;
using uint16 = std::uint16_t;
using int32 = std::int32_t;
using uint32 = std::uint32_t;
// ------------------- Enumerations ------------------- //
@ -97,11 +102,8 @@ constexpr decimal DEFAULT_BOUNCINESS = decimal(0.5);
/// Default rolling resistance
constexpr decimal DEFAULT_ROLLING_RESISTANCE = decimal(0.0);
/// True if the spleeping technique is enabled
constexpr bool SPLEEPING_ENABLED = true;
/// Object margin for collision detection in meters (for the GJK-EPA Algorithm)
constexpr decimal OBJECT_MARGIN = decimal(0.04);
/// True if the sleeping technique is enabled
constexpr bool SLEEPING_ENABLED = true;
/// Distance threshold for two contact points for a valid persistent contact (in meters)
constexpr decimal PERSISTENT_CONTACT_DIST_THRESHOLD = decimal(0.03);
@ -144,6 +146,14 @@ constexpr int NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1;
/// least one concave collision shape.
constexpr int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3;
/// This is used to test if two contact manifold are similar (same contact normal) in order to
/// merge them. If the cosine of the angle between the normals of the two manifold are larger
/// than the value bellow, the manifold are considered to be similar.
constexpr decimal COS_ANGLE_SIMILAR_CONTACT_MANIFOLD = decimal(0.95);
/// Size (in bytes) of the single frame allocator
constexpr size_t INIT_SINGLE_FRAME_ALLOCATOR_BYTES = 1048576; // 1Mb
}
#endif

4
src/constraint/BallAndSocketJoint.cpp
View File

@ -45,8 +45,8 @@ BallAndSocketJoint::BallAndSocketJoint(const BallAndSocketJointInfo& jointInfo)
void BallAndSocketJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Initialize the bodies index in the velocity array
mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
mIndexBody1 = mBody1->mArrayIndex;
mIndexBody2 = mBody2->mArrayIndex;
// Get the bodies center of mass and orientations
const Vector3& x1 = mBody1->mCenterOfMassWorld;

41
src/constraint/ContactPoint.cpp
View File

@ -31,23 +31,30 @@ using namespace reactphysics3d;
using namespace std;
// Constructor
ContactPoint::ContactPoint(const ContactPointInfo& contactInfo)
: mBody1(contactInfo.shape1->getBody()), mBody2(contactInfo.shape2->getBody()),
mNormal(contactInfo.normal),
mPenetrationDepth(contactInfo.penetrationDepth),
mLocalPointOnBody1(contactInfo.localPoint1),
mLocalPointOnBody2(contactInfo.localPoint2),
mWorldPointOnBody1(contactInfo.shape1->getBody()->getTransform() *
contactInfo.shape1->getLocalToBodyTransform() *
contactInfo.localPoint1),
mWorldPointOnBody2(contactInfo.shape2->getBody()->getTransform() *
contactInfo.shape2->getLocalToBodyTransform() *
contactInfo.localPoint2),
mIsRestingContact(false) {
ContactPoint::ContactPoint(const ContactPointInfo* contactInfo)
: mNormal(contactInfo->normal),
mPenetrationDepth(contactInfo->penetrationDepth),
mLocalPointOnShape1(contactInfo->localPoint1),
mLocalPointOnShape2(contactInfo->localPoint2),
mIsRestingContact(false), mIsObsolete(false), mNext(nullptr), mPrevious(nullptr) {
mFrictionVectors[0] = Vector3(0, 0, 0);
mFrictionVectors[1] = Vector3(0, 0, 0);
assert(mPenetrationDepth > 0.0);
assert(mPenetrationDepth > decimal(0.0));
assert(mNormal.lengthSquare() > decimal(0.8));
mIsObsolete = false;
}
// Update the contact point with a new one that is similar (very close)
/// The idea is to keep the cache impulse (for warm starting the contact solver)
void ContactPoint::update(const ContactPointInfo* contactInfo) {
assert(isSimilarWithContactPoint(contactInfo));
assert(contactInfo->penetrationDepth > decimal(0.0));
mNormal = contactInfo->normal;
mPenetrationDepth = contactInfo->penetrationDepth;
mLocalPointOnShape1 = contactInfo->localPoint1;
mLocalPointOnShape2 = contactInfo->localPoint2;
mIsObsolete = false;
}

308
src/constraint/ContactPoint.h
View File

@ -28,67 +28,14 @@
// Libraries
#include "body/CollisionBody.h"
#include "collision/CollisionShapeInfo.h"
#include "collision/ContactPointInfo.h"
#include "configuration.h"
#include "mathematics/mathematics.h"
#include "configuration.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Structure ContactPointInfo
/**
* This structure contains informations about a collision contact
* computed during the narrow-phase collision detection. Those
* informations are used to compute the contact set for a contact
* between two bodies.
*/
struct ContactPointInfo {
private:
// -------------------- Methods -------------------- //
public:
// -------------------- Attributes -------------------- //
/// First proxy shape of the contact
ProxyShape* shape1;
/// Second proxy shape of the contact
ProxyShape* shape2;
/// First collision shape
const CollisionShape* collisionShape1;
/// Second collision shape
const CollisionShape* collisionShape2;
/// Normalized normal vector of the collision contact in world space
Vector3 normal;
/// Penetration depth of the contact
decimal penetrationDepth;
/// Contact point of body 1 in local space of body 1
Vector3 localPoint1;
/// Contact point of body 2 in local space of body 2
Vector3 localPoint2;
// -------------------- Methods -------------------- //
/// Constructor
ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2, const CollisionShape* collShape1,
const CollisionShape* collShape2, const Vector3& normal, decimal penetrationDepth,
const Vector3& localPoint1, const Vector3& localPoint2)
: shape1(proxyShape1), shape2(proxyShape2), collisionShape1(collShape1), collisionShape2(collShape2),
normal(normal), penetrationDepth(penetrationDepth), localPoint1(localPoint1),
localPoint2(localPoint2) {
}
};
struct NarrowPhaseInfo;
// Class ContactPoint
/**
@ -101,54 +48,66 @@ class ContactPoint {
// -------------------- Attributes -------------------- //
/// First rigid body of the contact
CollisionBody* mBody1;
/// Second rigid body of the contact
CollisionBody* mBody2;
/// Normalized normal vector of the contact (from body1 toward body2) in world space
const Vector3 mNormal;
Vector3 mNormal;
/// Penetration depth
decimal mPenetrationDepth;
/// Contact point on body 1 in local space of body 1
const Vector3 mLocalPointOnBody1;
/// Contact point on proxy shape 1 in local-space of proxy shape 1
Vector3 mLocalPointOnShape1;
/// Contact point on body 2 in local space of body 2
const Vector3 mLocalPointOnBody2;
/// Contact point on body 1 in world space
Vector3 mWorldPointOnBody1;
/// Contact point on body 2 in world space
Vector3 mWorldPointOnBody2;
/// Contact point on proxy shape 2 in local-space of proxy shape 2
Vector3 mLocalPointOnShape2;
/// True if the contact is a resting contact (exists for more than one time step)
bool mIsRestingContact;
/// Two orthogonal vectors that span the tangential friction plane
Vector3 mFrictionVectors[2];
/// Cached penetration impulse
decimal mPenetrationImpulse;
/// Cached first friction impulse
decimal mFrictionImpulse1;
/// True if the contact point is obsolete
bool mIsObsolete;
/// Cached second friction impulse
decimal mFrictionImpulse2;
/// Pointer to the next contact point in the double linked-list
ContactPoint* mNext;
/// Cached rolling resistance impulse
Vector3 mRollingResistanceImpulse;
/// Pointer to the previous contact point in the double linked-list
ContactPoint* mPrevious;
// -------------------- Methods -------------------- //
/// Update the contact point with a new one that is similar (very close)
void update(const ContactPointInfo* contactInfo);
/// Return true if the contact point is similar (close enougth) to another given contact point
bool isSimilarWithContactPoint(const ContactPointInfo* contactPoint) const;
/// Set the cached penetration impulse
void setPenetrationImpulse(decimal impulse);
/// Set the mIsRestingContact variable
void setIsRestingContact(bool isRestingContact);
/// Set to true to make the contact point obsolete
void setIsObsolete(bool isObselete);
/// Set the next contact point in the linked list
void setNext(ContactPoint* next);
/// Set the previous contact point in the linked list
void setPrevious(ContactPoint* previous);
/// Return true if the contact point is obsolete
bool getIsObsolete() const;
public :
// -------------------- Methods -------------------- //
/// Constructor
ContactPoint(const ContactPointInfo& contactInfo);
ContactPoint(const ContactPointInfo* contactInfo);
/// Destructor
~ContactPoint() = default;
@ -159,123 +118,52 @@ class ContactPoint {
/// Deleted assignment operator
ContactPoint& operator=(const ContactPoint& contact) = delete;
/// Return the reference to the body 1
CollisionBody* getBody1() const;
/// Return the reference to the body 2
CollisionBody* getBody2() const;
/// Return the normal vector of the contact
Vector3 getNormal() const;
/// Set the penetration depth of the contact
void setPenetrationDepth(decimal penetrationDepth);
/// Return the contact point on the first proxy shape in the local-space of the proxy shape
const Vector3& getLocalPointOnShape1() const;
/// Return the contact local point on body 1
Vector3 getLocalPointOnBody1() const;
/// Return the contact local point on body 2
Vector3 getLocalPointOnBody2() const;
/// Return the contact world point on body 1
Vector3 getWorldPointOnBody1() const;
/// Return the contact world point on body 2
Vector3 getWorldPointOnBody2() const;
/// Return the contact point on the second proxy shape in the local-space of the proxy shape
const Vector3& getLocalPointOnShape2() const;
/// Return the cached penetration impulse
decimal getPenetrationImpulse() const;
/// Return the cached first friction impulse
decimal getFrictionImpulse1() const;
/// Return the cached second friction impulse
decimal getFrictionImpulse2() const;
/// Return the cached rolling resistance impulse
Vector3 getRollingResistanceImpulse() const;
/// Set the cached penetration impulse
void setPenetrationImpulse(decimal impulse);
/// Set the first cached friction impulse
void setFrictionImpulse1(decimal impulse);
/// Set the second cached friction impulse
void setFrictionImpulse2(decimal impulse);
/// Set the cached rolling resistance impulse
void setRollingResistanceImpulse(const Vector3& impulse);
/// Set the contact world point on body 1
void setWorldPointOnBody1(const Vector3& worldPoint);
/// Set the contact world point on body 2
void setWorldPointOnBody2(const Vector3& worldPoint);
/// Return true if the contact is a resting contact
bool getIsRestingContact() const;
/// Set the mIsRestingContact variable
void setIsRestingContact(bool isRestingContact);
/// Return the previous contact point in the linked list
inline ContactPoint* getPrevious() const;
/// Get the first friction vector
Vector3 getFrictionVector1() const;
/// Set the first friction vector
void setFrictionVector1(const Vector3& frictionVector1);
/// Get the second friction vector
Vector3 getFrictionVector2() const;
/// Set the second friction vector
void setFrictionVector2(const Vector3& frictionVector2);
/// Return the next contact point in the linked list
ContactPoint* getNext() const;
/// Return the penetration depth
decimal getPenetrationDepth() const;
/// Return the number of bytes used by the contact point
size_t getSizeInBytes() const;
// Friendship
friend class ContactManifold;
friend class ContactManifoldSet;
friend class ContactSolver;
};
// Return the reference to the body 1
inline CollisionBody* ContactPoint::getBody1() const {
return mBody1;
}
// Return the reference to the body 2
inline CollisionBody* ContactPoint::getBody2() const {
return mBody2;
}
// Return the normal vector of the contact
inline Vector3 ContactPoint::getNormal() const {
return mNormal;
}
// Set the penetration depth of the contact
inline void ContactPoint::setPenetrationDepth(decimal penetrationDepth) {
this->mPenetrationDepth = penetrationDepth;
// Return the contact point on the first proxy shape in the local-space of the proxy shape
inline const Vector3& ContactPoint::getLocalPointOnShape1() const {
return mLocalPointOnShape1;
}
// Return the contact point on body 1
inline Vector3 ContactPoint::getLocalPointOnBody1() const {
return mLocalPointOnBody1;
}
// Return the contact point on body 2
inline Vector3 ContactPoint::getLocalPointOnBody2() const {
return mLocalPointOnBody2;
}
// Return the contact world point on body 1
inline Vector3 ContactPoint::getWorldPointOnBody1() const {
return mWorldPointOnBody1;
}
// Return the contact world point on body 2
inline Vector3 ContactPoint::getWorldPointOnBody2() const {
return mWorldPointOnBody2;
// Return the contact point on the second proxy shape in the local-space of the proxy shape
inline const Vector3& ContactPoint::getLocalPointOnShape2() const {
return mLocalPointOnShape2;
}
// Return the cached penetration impulse
@ -283,19 +171,10 @@ inline decimal ContactPoint::getPenetrationImpulse() const {
return mPenetrationImpulse;
}
// Return the cached first friction impulse
inline decimal ContactPoint::getFrictionImpulse1() const {
return mFrictionImpulse1;
}
// Return the cached second friction impulse
inline decimal ContactPoint::getFrictionImpulse2() const {
return mFrictionImpulse2;
}
// Return the cached rolling resistance impulse
inline Vector3 ContactPoint::getRollingResistanceImpulse() const {
return mRollingResistanceImpulse;
// Return true if the contact point is similar (close enougth) to another given contact point
inline bool ContactPoint::isSimilarWithContactPoint(const ContactPointInfo* localContactPointBody1) const {
return (localContactPointBody1->localPoint1 - mLocalPointOnShape1).lengthSquare() <= (PERSISTENT_CONTACT_DIST_THRESHOLD *
PERSISTENT_CONTACT_DIST_THRESHOLD);
}
// Set the cached penetration impulse
@ -303,31 +182,6 @@ inline void ContactPoint::setPenetrationImpulse(decimal impulse) {
mPenetrationImpulse = impulse;
}
// Set the first cached friction impulse
inline void ContactPoint::setFrictionImpulse1(decimal impulse) {
mFrictionImpulse1 = impulse;
}
// Set the second cached friction impulse
inline void ContactPoint::setFrictionImpulse2(decimal impulse) {
mFrictionImpulse2 = impulse;
}
// Set the cached rolling resistance impulse
inline void ContactPoint::setRollingResistanceImpulse(const Vector3& impulse) {
mRollingResistanceImpulse = impulse;
}
// Set the contact world point on body 1
inline void ContactPoint::setWorldPointOnBody1(const Vector3& worldPoint) {
mWorldPointOnBody1 = worldPoint;
}
// Set the contact world point on body 2
inline void ContactPoint::setWorldPointOnBody2(const Vector3& worldPoint) {
mWorldPointOnBody2 = worldPoint;
}
// Return true if the contact is a resting contact
inline bool ContactPoint::getIsRestingContact() const {
return mIsRestingContact;
@ -338,24 +192,34 @@ inline void ContactPoint::setIsRestingContact(bool isRestingContact) {
mIsRestingContact = isRestingContact;
}
// Get the first friction vector
inline Vector3 ContactPoint::getFrictionVector1() const {
return mFrictionVectors[0];
// Return true if the contact point is obsolete
inline bool ContactPoint::getIsObsolete() const {
return mIsObsolete;
}
// Set the first friction vector
inline void ContactPoint::setFrictionVector1(const Vector3& frictionVector1) {
mFrictionVectors[0] = frictionVector1;
// Set to true to make the contact point obsolete
inline void ContactPoint::setIsObsolete(bool isObsolete) {
mIsObsolete = isObsolete;
}
// Get the second friction vector
inline Vector3 ContactPoint::getFrictionVector2() const {
return mFrictionVectors[1];
// Return the next contact point in the linked list
inline ContactPoint* ContactPoint::getNext() const {
return mNext;
}
// Set the second friction vector
inline void ContactPoint::setFrictionVector2(const Vector3& frictionVector2) {
mFrictionVectors[1] = frictionVector2;
// Set the next contact point in the linked list
inline void ContactPoint::setNext(ContactPoint* next) {
mNext = next;
}
// Return the previous contact point in the linked list
inline ContactPoint* ContactPoint::getPrevious() const {
return mPrevious;
}
// Set the previous contact point in the linked list
inline void ContactPoint::setPrevious(ContactPoint* previous) {
mPrevious = previous;
}
// Return the penetration depth of the contact

4
src/constraint/FixedJoint.cpp
View File

@ -60,8 +60,8 @@ FixedJoint::FixedJoint(const FixedJointInfo& jointInfo)
void FixedJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Initialize the bodies index in the velocity array
mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
mIndexBody1 = mBody1->mArrayIndex;
mIndexBody2 = mBody2->mArrayIndex;
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->mCenterOfMassWorld;

4
src/constraint/HingeJoint.cpp
View File

@ -68,8 +68,8 @@ HingeJoint::HingeJoint(const HingeJointInfo& jointInfo)
void HingeJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Initialize the bodies index in the velocity array
mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
mIndexBody1 = mBody1->mArrayIndex;
mIndexBody2 = mBody2->mArrayIndex;
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->mCenterOfMassWorld;

4
src/constraint/SliderJoint.cpp
View File

@ -74,8 +74,8 @@ SliderJoint::SliderJoint(const SliderJointInfo& jointInfo)
void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverData) {
// Initialize the bodies index in the veloc ity array
mIndexBody1 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody1)->second;
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
mIndexBody1 = mBody1->mArrayIndex;
mIndexBody2 = mBody2->mArrayIndex;
// Get the bodies positions and orientations
const Vector3& x1 = mBody1->mCenterOfMassWorld;

124
src/containers/LinkedList.h Normal file
View File

@ -0,0 +1,124 @@
/********************************************************************************
* 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_LINKED_LIST_H
#define REACTPHYSICS3D_LINKED_LIST_H
// Libraries
#include "memory/MemoryAllocator.h"
namespace reactphysics3d {
// Class LinkedList
/**
* This class represents a simple generic linked list.
*/
template<typename T>
class LinkedList {
public:
/// Element of the linked list
struct ListElement {
/// Data of the list element
T data;
/// Pointer to the next element of the list
ListElement* next;
/// Constructor
ListElement(T elementData, ListElement* nextElement)
: data(elementData), next(nextElement) {
}
};
private:
// -------------------- Attributes -------------------- //
/// Pointer to the first element of the list
ListElement* mListHead;
/// Memory allocator used to allocate the list elements
MemoryAllocator& mAllocator;
public:
// -------------------- Methods -------------------- //
/// Constructor
LinkedList(MemoryAllocator& allocator) : mListHead(nullptr), mAllocator(allocator) {
}
/// Destructor
~LinkedList() {
reset();
}
/// Return the first element of the list
ListElement* getListHead() const;
/// Insert an element at the beginning of the linked list
void insert(const T& data);
/// Remove all the elements of the list
void reset();
};
// Return the first element of the list
template<typename T>
inline typename LinkedList<T>::ListElement* LinkedList<T>::getListHead() const {
return mListHead;
}
// Insert an element at the beginning of the linked list
template<typename T>
inline void LinkedList<T>::insert(const T& data) {
ListElement* element = new (mAllocator.allocate(sizeof(ListElement))) ListElement(data, mListHead);
mListHead = element;
}
// Remove all the elements of the list
template<typename T>
inline void LinkedList<T>::reset() {
// Release all the list elements
ListElement* element = mListHead;
while (element != nullptr) {
ListElement* nextElement = element->next;
mAllocator.release(element, sizeof(ListElement));
element = nextElement;
}
mListHead = nullptr;
}
}
#endif

341
src/containers/List.h Normal file
View File

@ -0,0 +1,341 @@
/********************************************************************************
* 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_LIST_H
#define REACTPHYSICS3D_LIST_H
// Libraries
#include "configuration.h"
#include "memory/MemoryAllocator.h"
#include <cstring>
#include <iterator>
namespace reactphysics3d {
// Class List
/**
* This class represents a simple generic list with custom memory allocator.
*/
template<typename T>
class List {
private:
// -------------------- Attributes -------------------- //
/// Buffer for the list elements
void* mBuffer;
/// Number of elements in the list
size_t mSize;
/// Number of allocated elements in the list
size_t mCapacity;
/// Memory allocator
MemoryAllocator& mAllocator;
// -------------------- Methods -------------------- //
public:
/// Class Iterator
/**
* This class represents an iterator for the List
*/
class Iterator {
private:
size_t mCurrentIndex;
T* mBuffer;
size_t mSize;
public:
// Iterator traits
using value_type = T;
using difference_type = std::ptrdiff_t;
using pointer = T*;
using reference = T&;
using iterator_category = std::bidirectional_iterator_tag;
/// Constructor
Iterator() = default;
/// Constructor
Iterator(T* buffer, size_t index, size_t size)
:mCurrentIndex(index), mBuffer(buffer), mSize(size) {
}
/// Copy constructor
Iterator(const Iterator& it)
:mCurrentIndex(it.mCurrentIndex), mBuffer(it.mBuffer), mSize(it.size) {
}
/// Deferencable
reference operator*() const {
assert(mCurrentIndex >= 0 && mCurrentIndex < mSize);
return mBuffer[mCurrentIndex];
}
/// Deferencable
pointer operator->() const {
assert(mCurrentIndex >= 0 && mCurrentIndex < mSize);
return &(mBuffer[mCurrentIndex]);
}
/// Post increment (it++)
Iterator& operator++() {
assert(mCurrentIndex < mSize - 1);
mCurrentIndex++;
return *this;
}
/// Pre increment (++it)
Iterator operator++(int number) {
assert(mCurrentIndex < mSize - 1);
Iterator tmp = *this;
mCurrentIndex++;
return tmp;
}
/// Post decrement (it--)
Iterator& operator--() {
assert(mCurrentIndex > 0);
mCurrentIndex--;
return *this;
}
/// Pre decrement (--it)
Iterator operator--(int number) {
assert(mCurrentIndex > 0);
Iterator tmp = *this;
mCurrentIndex--;
return tmp;
}
/// Equality operator (it == end())
bool operator==(const Iterator& iterator) const {
assert(mCurrentIndex >= 0 && mCurrentIndex <= mSize);
// If both iterators points to the end of the list
if (mCurrentIndex == mSize && iterator.mCurrentIndex == iterator.mSize) {
return true;
}
return &(mBuffer[mCurrentIndex]) == &(iterator.mBuffer[mCurrentIndex]);
}
/// Inequality operator (it != end())
bool operator!=(const Iterator& iterator) const {
return !(*this == iterator);
}
};
// -------------------- Methods -------------------- //
/// Constructor
List(MemoryAllocator& allocator, size_t capacity = 0)
: mBuffer(nullptr), mSize(0), mCapacity(0), mAllocator(allocator) {
if (capacity > 0) {
// Allocate memory
reserve(capacity);
}
}
/// Copy constructor
List(const List<T>& list) : mBuffer(nullptr), mSize(0), mCapacity(0), mAllocator(list.mAllocator) {
// All all the elements of the list to the current one
addRange(list);
}
/// Destructor
~List() {
// If elements have been allocated
if (mCapacity > 0) {
// Clear the list
clear();
// Release the memory allocated on the heap
mAllocator.release(mBuffer, mCapacity * sizeof(T));
}
}
/// Allocate memory for a given number of elements
void reserve(size_t capacity) {
if (capacity <= mCapacity) return;
// Allocate memory for the new array
void* newMemory = mAllocator.allocate(capacity * sizeof(T));
if (mBuffer != nullptr) {
// Copy the elements to the new allocated memory location
std::memcpy(newMemory, mBuffer, mSize * sizeof(T));
// Release the previously allocated memory
mAllocator.release(mBuffer, mCapacity * sizeof(T));
}
mBuffer = newMemory;
assert(mBuffer != nullptr);
mCapacity = capacity;
}
/// Add an element into the list
void add(const T& element) {
// If we need to allocate more memory
if (mSize == mCapacity) {
reserve(mCapacity == 0 ? 1 : mCapacity * 2);
}
// Use the copy-constructor to construct the element
new (static_cast<char*>(mBuffer) + mSize * sizeof(T)) T(element);
mSize++;
}
/// Remove an element from the list at a given index
void remove(uint index) {
assert(index >= 0 && index < mSize);
// Call the destructor
(static_cast<T*>(mBuffer)[index]).~T();
mSize--;
if (index != mSize) {
// Move the elements to fill in the empty slot
char* dest = static_cast<char*>(mBuffer) + index * sizeof(T);
char* src = dest + sizeof(T);
std::memcpy(static_cast<void*>(dest), static_cast<void*>(src), (mSize - index) * sizeof(T));
}
}
/// Append another list at the end of the current one
void addRange(const List<T>& list) {
// If we need to allocate more memory
if (mSize + list.size() > mCapacity) {
// Allocate memory
reserve(mSize + list.size());
}
// Add the elements of the list to the current one
for(uint i=0; i<list.size(); i++) {
new (static_cast<char*>(mBuffer) + mSize * sizeof(T)) T(list[i]);
mSize++;
}
}
/// Clear the list
void clear() {
// Call the destructor of each element
for (uint i=0; i < mSize; i++) {
(static_cast<T*>(mBuffer)[i]).~T();
}
mSize = 0;
}
/// Return the number of elements in the list
size_t size() const {
return mSize;
}
/// Return the capacity of the list
size_t capacity() const {
return mCapacity;
}
/// Overloaded index operator
T& operator[](const uint index) {
assert(index >= 0 && index < mSize);
return (static_cast<T*>(mBuffer)[index]);
}
/// Overloaded const index operator
const T& operator[](const uint index) const {
assert(index >= 0 && index < mSize);
return (static_cast<T*>(mBuffer)[index]);
}
/// Overloaded equality operator
bool operator==(const List<T>& list) const {
if (mSize != list.mSize) return false;
T* items = static_cast<T*>(mBuffer);
for (int i=0; i < mSize; i++) {
if (items[i] != list[i]) {
return false;
}
}
return true;
}
/// Overloaded not equal operator
bool operator!=(const List<T>& list) const {
return !((*this) == list);
}
/// Overloaded assignment operator
List<T>& operator=(const List<T>& list) {
if (this != &list) {
// Clear all the elements
clear();
// Add all the elements of the list to the current one
addRange(list);
}
return *this;
}
};
}
#endif

532
src/containers/Map.h Normal file
View File

@ -0,0 +1,532 @@
/********************************************************************************
* 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_MAP_H
#define REACTPHYSICS3D_MAP_H
// Libraries
#include "memory/MemoryAllocator.h"
#include "mathematics/mathematics_functions.h"
#include <cstring>
#include <limits>
namespace reactphysics3d {
// Class Map
/**
* This class represents a simple generic associative map
*/
template<typename K, typename V>
class Map {
private:
/// An entry of the map
struct Entry {
size_t hashCode; // Hash code of the entry
int next; // Index of the next entry
std::pair<K, V>* keyValue; // Pointer to the pair with key and value
/// Constructor
Entry() {
next = -1;
keyValue = nullptr;
}
/// Destructor
~Entry() {
assert(keyValue == nullptr);
}
};
// -------------------- Constants -------------------- //
/// Number of prime numbers in array
static constexpr int NB_PRIMES = 70;
/// Array of prime numbers for the size of the map
static const int PRIMES[NB_PRIMES];
/// Largest prime number
static int LARGEST_PRIME;
// -------------------- Attributes -------------------- //
/// Current number of used entries in the map
int mNbUsedEntries;
/// Number of free entries among the used ones
int mNbFreeEntries;
/// Current capacity of the map
int mCapacity;
/// Array with all the buckets
int* mBuckets;
/// Array with all the entries
Entry* mEntries;
/// Memory allocator
MemoryAllocator& mAllocator;
/// Index to the fist free entry
int mFreeIndex;
// -------------------- Methods -------------------- //
/// Initialize the map
void initialize(int capacity) {
// Compute the next larger prime size
mCapacity = getPrimeSize(capacity);
// Allocate memory for the buckets
mBuckets = static_cast<int*>(mAllocator.allocate(mCapacity * sizeof(int)));
// Allocate memory for the entries
mEntries = static_cast<Entry*>(mAllocator.allocate(mCapacity * sizeof(Entry)));
// Initialize the buckets and entries
for (int i=0; i<mCapacity; i++) {
mBuckets[i] = -1;
// Construct the entry
new (&mEntries[i]) Entry();
}
mNbUsedEntries = 0;
mNbFreeEntries = 0;
mFreeIndex = -1;
}
/// Expand the capacity of the map
void expand(int newCapacity) {
assert(newCapacity > mCapacity);
assert(isPrimeNumber(newCapacity));
// Allocate memory for the buckets
int* newBuckets = static_cast<int*>(mAllocator.allocate(newCapacity * sizeof(int)));
// Allocate memory for the entries
Entry* newEntries = static_cast<Entry*>(mAllocator.allocate(newCapacity * sizeof(Entry)));
// Initialize the new buckets
for (int i=0; i<newCapacity; i++) {
newBuckets[i] = -1;
}
// Copy the old entries to the new allocated memory location
std::memcpy(newEntries, mEntries, mNbUsedEntries * sizeof(Entry));
// Construct the new entries
for (int i=mNbUsedEntries; i<newCapacity; i++) {
// Construct the entry
new (static_cast<void*>(&newEntries[i])) Entry();
}
// For each used entry
for (int i=0; i<mNbUsedEntries; i++) {
// If the entry is not free
if (newEntries[i].hashCode != -1) {
// Get the corresponding bucket
int bucket = newEntries[i].hashCode % newCapacity;
newEntries[i].next = newBuckets[bucket];
newBuckets[bucket] = i;
}
}
// Release previously allocated memory
mAllocator.release(mBuckets, mCapacity * sizeof(int));
mAllocator.release(mEntries, mCapacity * sizeof(Entry));
mCapacity = newCapacity;
mBuckets = newBuckets;
mEntries = newEntries;
}
/// Return the index of the entry with a given key or -1 if there is no entry with this key
int findEntry(const K& key) const {
if (mCapacity > 0) {
size_t hashCode = std::hash<K>()(key);
int bucket = hashCode % mCapacity;
for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) {
if (mEntries[i].hashCode == hashCode && mEntries[i].keyValue->first == key) {
return i;
}
}
}
return -1;
}
/// Return the prime number that is larger or equal to the number in parameter
/// for the size of the map
static int getPrimeSize(int number) {
// Check if the next larger prime number is in the precomputed array of primes
for (int i = 0; i < NB_PRIMES; i++) {
if (PRIMES[i] >= number) return PRIMES[i];
}
// Manually compute the next larger prime number
for (int i = (number | 1); i < std::numeric_limits<int>::max(); i+=2) {
if (isPrimeNumber(i)) {
return i;
}
}
return number;
}
/// Clear and reset the map
void reset() {
// If elements have been allocated
if (mCapacity > 0) {
// Clear the list
clear();
// Destroy the entries
for (int i=0; i < mCapacity; i++) {
mEntries[i].~Entry();
}
mAllocator.release(mBuckets, mCapacity * sizeof(int));
mAllocator.release(mEntries, mCapacity * sizeof(Entry));
mNbUsedEntries = 0;
mNbFreeEntries = 0;
mCapacity = 0;
mBuckets = nullptr;
mEntries = nullptr;
mFreeIndex = -1;
}
}
public:
// -------------------- Methods -------------------- //
/// Constructor
Map(MemoryAllocator& allocator, size_t capacity = 0)
: mNbUsedEntries(0), mNbFreeEntries(0), mCapacity(0), mBuckets(nullptr),
mEntries(nullptr), mAllocator(allocator), mFreeIndex(-1) {
// If the largest prime has not been computed yet
if (LARGEST_PRIME == -1) {
// Compute the largest prime number (largest map capacity)
LARGEST_PRIME = getPrimeSize(PRIMES[NB_PRIMES - 1] + 2);
}
if (capacity > 0) {
initialize(capacity);
}
}
/// Copy constructor
Map(const Map<K, V>& map)
:mNbUsedEntries(map.mNbUsedEntries), mNbFreeEntries(map.mNbFreeEntries), mCapacity(map.mCapacity),
mAllocator(map.mAllocator), mFreeIndex(map.mFreeIndex) {
// Allocate memory for the buckets
mBuckets = static_cast<int*>(mAllocator.allocate(mCapacity * sizeof(int)));
// Allocate memory for the entries
mEntries = static_cast<Entry*>(mAllocator.allocate(mCapacity * sizeof(Entry)));
// Copy the buckets
std::memcpy(mBuckets, map.mBuckets, mCapacity * sizeof(int));
// Copy the entries
std::memcpy(mEntries, map.mEntries, mCapacity * sizeof(Entry));
}
/// Destructor
~Map() {
reset();
}
/// Allocate memory for a given number of elements
void reserve(size_t capacity) {
if (capacity <= mCapacity) return;
if (capacity > LARGEST_PRIME && LARGEST_PRIME > mCapacity) {
capacity = LARGEST_PRIME;
}
else {
capacity = getPrimeSize(capacity);
}
expand(capacity);
}
/// Return true if the map contains an item with the given key
bool containsKey(const K& key) const {
return findEntry(key) != -1;
}
/// Add an element into the map
void add(const std::pair<K,V>& keyValue) {
if (mCapacity == 0) {
initialize(0);
}
// Compute the hash code of the key
size_t hashCode = std::hash<K>()(keyValue.first);
// Compute the corresponding bucket index
int bucket = hashCode % mCapacity;
// Check if the item is already in the map
for (int i = mBuckets[bucket]; i >= 0; i = mEntries[i].next) {
// If there is already an item with the same key in the map
if (mEntries[i].hashCode == hashCode && mEntries[i].keyValue->first == keyValue.first) {
throw std::runtime_error("The key and value pair already exists in the map");
}
}
size_t entryIndex;
// If there are free entries to use
if (mNbFreeEntries > 0) {
assert(mFreeIndex >= 0);
entryIndex = mFreeIndex;
mFreeIndex = mEntries[entryIndex].next;
mNbFreeEntries--;
}
else {
// If we need to allocator more entries
if (mNbUsedEntries == mCapacity) {
// Allocate more memory
reserve(mCapacity * 2);
// Recompute the bucket index
bucket = hashCode % mCapacity;
}
entryIndex = mNbUsedEntries;
mNbUsedEntries++;
}
assert(mEntries[entryIndex].keyValue == nullptr);
mEntries[entryIndex].hashCode = hashCode;
mEntries[entryIndex].next = mBuckets[bucket];
mEntries[entryIndex].keyValue = static_cast<std::pair<K,V>*>(mAllocator.allocate(sizeof(std::pair<K,V>)));
assert(mEntries[entryIndex].keyValue != nullptr);
new (mEntries[entryIndex].keyValue) std::pair<K,V>(keyValue);
mBuckets[bucket] = entryIndex;
}
/// Remove the element from the map with a given key
bool remove(const K& key) {
if (mCapacity > 0) {
size_t hashcode = std::hash<K>()(key);
int bucket = hashcode % mCapacity;
int last = -1;
for (int i = mBuckets[bucket]; i >= 0; last = i, i = mEntries[i].next) {
if (mEntries[i].hashCode == hashcode && mEntries[i].keyValue->first == key) {
if (last < 0 ) {
mBuckets[bucket] = mEntries[i].next;
}
else {
mEntries[last].next = mEntries[i].next;
}
// Release memory for the key/value pair if any
if (mEntries[i].keyValue != nullptr) {
mEntries[i].keyValue->~pair<K,V>();
mAllocator.release(mEntries[i].keyValue, sizeof(std::pair<K,V>));
mEntries[i].keyValue = nullptr;
}
mEntries[i].hashCode = -1;
mEntries[i].next = mFreeIndex;
mFreeIndex = i;
mNbFreeEntries++;
return true;
}
}
}
return false;
}
/// Clear the list
void clear() {
if (mNbUsedEntries > 0) {
for (int i=0; i < mCapacity; i++) {
mBuckets[i] = -1;
mEntries[i].next = -1;
if (mEntries[i].keyValue != nullptr) {
mEntries[i].keyValue->~pair<K,V>();
mAllocator.release(mEntries[i].keyValue, sizeof(std::pair<K,V>));
mEntries[i].keyValue = nullptr;
}
}
mFreeIndex = -1;
mNbUsedEntries = 0;
mNbFreeEntries = 0;
}
assert(size() == 0);
}
/// Return the number of elements in the map
int size() const {
return mNbUsedEntries - mNbFreeEntries;
}
/// Return the capacity of the map
int capacity() const {
return mCapacity;
}
/// Overloaded index operator
V& operator[](const K& key) {
int entry = -1;
if (mCapacity > 0) {
entry = findEntry(key);
}
if (entry == -1) {
throw std::runtime_error("No item with given key has been found in the map");
}
assert(mEntries[entry].keyValue != nullptr);
return mEntries[entry].keyValue->second;
}
/// Overloaded index operator
const V& operator[](const K& key) const {
int entry = -1;
if (mCapacity > 0) {
entry = findEntry(key);
}
if (entry == -1) {
throw std::runtime_error("No item with given key has been found in the map");
}
return mEntries[entry];
}
/// Overloaded equality operator
bool operator==(const Map<K,V>& map) const {
// TODO : Implement this
return false;
}
/// Overloaded not equal operator
bool operator!=(const Map<K,V>& map) const {
return !((*this) == map);
}
/// Overloaded assignment operator
Map<K,V>& operator=(const Map<K, V>& map) {
// Check for self assignment
if (this != &map) {
// Reset the map
reset();
if (map.mCapacity > 0) {
// Compute the next larger prime size
mCapacity = getPrimeSize(map.mCapacity);
// Allocate memory for the buckets
mBuckets = static_cast<int*>(mAllocator.allocate(mCapacity * sizeof(int)));
// Allocate memory for the entries
mEntries = static_cast<Entry*>(mAllocator.allocate(mCapacity * sizeof(Entry)));
// Copy the buckets
std::memcpy(mBuckets, map.mBuckets, mCapacity * sizeof(int));
// Copy the entries
std::memcpy(mEntries, map.mEntries, mCapacity * sizeof(Entry));
mNbUsedEntries = map.mNbUsedEntries;
mNbFreeEntries = map.mNbFreeEntries;
mFreeIndex = map.mFreeIndex;
}
}
return *this;
}
};
template<typename K, typename V>
const int Map<K,V>::PRIMES[NB_PRIMES] = {3, 7, 11, 17, 23, 29, 37, 47, 59, 71, 89, 107, 131, 163, 197, 239, 293, 353, 431, 521, 631, 761, 919,
1103, 1327, 1597, 1931, 2333, 2801, 3371, 4049, 4861, 5839, 7013, 8419, 10103, 12143, 14591,
17519, 21023, 25229, 30293, 36353, 43627, 52361, 62851, 75431, 90523, 108631, 130363, 156437,
187751, 225307, 270371, 324449, 389357, 467237, 560689, 672827, 807403, 968897, 1162687, 1395263,
1674319, 2009191, 2411033, 2893249, 3471899, 4166287, 4999559};
template<typename K, typename V>
int Map<K,V>::LARGEST_PRIME = -1;
}
#endif

Some files were not shown because too many files have changed in this diff Show More