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

Fix conflicts when merging triangularmeshes into develop

Browse Source
This commit is contained in:
Daniel Chappuis 2016-02-15 20:14:30 +01:00
commit d6580f94aa
124 changed files with 88294 additions and 1886 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
CMakeLists.txt
View File

@ -1,5 +1,5 @@
# Minimum cmake version required
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
CMAKE_MINIMUM_REQUIRED(VERSION 3.1.0 FATAL_ERROR)
# Project configuration
PROJECT(REACTPHYSICS3D)
@ -53,6 +53,9 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/broadphase/BroadPhaseAlgorithm.cpp"
"src/collision/broadphase/DynamicAABBTree.h"
"src/collision/broadphase/DynamicAABBTree.cpp"
"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"
@ -69,8 +72,14 @@ SET (REACTPHYSICS3D_SOURCES
"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/shapes/AABB.h"
"src/collision/shapes/AABB.cpp"
"src/collision/shapes/ConvexShape.h"
"src/collision/shapes/ConvexShape.cpp"
"src/collision/shapes/ConcaveShape.h"
"src/collision/shapes/ConcaveShape.cpp"
"src/collision/shapes/BoxShape.h"
"src/collision/shapes/BoxShape.cpp"
"src/collision/shapes/CapsuleShape.h"
@ -85,12 +94,27 @@ SET (REACTPHYSICS3D_SOURCES
"src/collision/shapes/CylinderShape.cpp"
"src/collision/shapes/SphereShape.h"
"src/collision/shapes/SphereShape.cpp"
"src/collision/shapes/TriangleShape.h"
"src/collision/shapes/TriangleShape.cpp"
"src/collision/shapes/ConcaveMeshShape.h"
"src/collision/shapes/ConcaveMeshShape.cpp"
"src/collision/shapes/HeightFieldShape.h"
"src/collision/shapes/HeightFieldShape.cpp"
"src/collision/RaycastInfo.h"
"src/collision/RaycastInfo.cpp"
"src/collision/ProxyShape.h"
"src/collision/ProxyShape.cpp"
"src/collision/TriangleVertexArray.h"
"src/collision/TriangleVertexArray.cpp"
"src/collision/TriangleMesh.h"
"src/collision/TriangleMesh.cpp"
"src/collision/CollisionDetection.h"
"src/collision/CollisionDetection.cpp"
"src/collision/CollisionShapeInfo.h"
"src/collision/ContactManifold.h"
"src/collision/ContactManifold.cpp"
"src/collision/ContactManifoldSet.h"
"src/collision/ContactManifoldSet.cpp"
"src/constraint/BallAndSocketJoint.h"
"src/constraint/BallAndSocketJoint.cpp"
"src/constraint/ContactPoint.h"
@ -107,8 +131,6 @@ SET (REACTPHYSICS3D_SOURCES
"src/engine/CollisionWorld.cpp"
"src/engine/ConstraintSolver.h"
"src/engine/ConstraintSolver.cpp"
"src/engine/ContactManifold.h"
"src/engine/ContactManifold.cpp"
"src/engine/ContactSolver.h"
"src/engine/ContactSolver.cpp"
"src/engine/DynamicsWorld.h"
@ -123,8 +145,11 @@ SET (REACTPHYSICS3D_SOURCES
"src/engine/OverlappingPair.cpp"
"src/engine/Profiler.h"
"src/engine/Profiler.cpp"
"src/engine/Timer.h"
"src/engine/Timer.cpp"
"src/mathematics/mathematics.h"
"src/mathematics/mathematics_functions.h"
"src/mathematics/mathematics_functions.cpp"
"src/mathematics/Matrix2x2.h"
"src/mathematics/Matrix2x2.cpp"
"src/mathematics/Matrix3x3.h"
@ -144,7 +169,11 @@ SET (REACTPHYSICS3D_SOURCES
)
# Create the library
ADD_LIBRARY (reactphysics3d STATIC ${REACTPHYSICS3D_SOURCES})
ADD_LIBRARY(reactphysics3d STATIC ${REACTPHYSICS3D_SOURCES})
# Enable C++11 features
set_property(TARGET reactphysics3d PROPERTY CXX_STANDARD 11)
set_property(TARGET reactphysics3d PROPERTY CXX_STANDARD_REQUIRED ON)
# If we need to compile the testbed application
IF(COMPILE_TESTBED)

43
src/body/CollisionBody.cpp
View File

@ -26,7 +26,7 @@
// Libraries
#include "CollisionBody.h"
#include "engine/CollisionWorld.h"
#include "engine/ContactManifold.h"
#include "collision/ContactManifold.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
@ -51,30 +51,27 @@ CollisionBody::~CollisionBody() {
removeAllCollisionShapes();
}
// Add a collision shape to the body.
/// When you add a collision shape to the body, an internal copy of this
/// collision shape will be created internally. Therefore, you can delete it
/// right after calling this method or use it later to add it to another body.
// Add a collision shape to the body. Note that you can share a collision
// shape between several bodies using the same collision shape instance to
// when you add the shape to the different bodies. Do not forget to delete
// the collision shape you have created at the end of your program.
/// This method will return a pointer to a new proxy shape. A proxy shape is
/// an object that links a collision shape and a given body. You can use the
/// returned proxy shape to get and set information about the corresponding
/// collision shape for that body.
/**
* @param collisionShape The collision shape you want to add to the body
* @param collisionShape A pointer to the collision shape you want to add to the body
* @param transform The transformation of the collision shape that transforms the
* local-space of the collision shape into the local-space of the body
* @return A pointer to the proxy shape that has been created to link the body to
* the new collision shape you have added.
*/
ProxyShape* CollisionBody::addCollisionShape(const CollisionShape& collisionShape,
ProxyShape* CollisionBody::addCollisionShape(CollisionShape* collisionShape,
const Transform& transform) {
// Create an internal copy of the collision shape into the world (if it does not exist yet)
CollisionShape* newCollisionShape = mWorld.createCollisionShape(collisionShape);
// Create a new proxy collision shape to attach the collision shape to the body
ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate(
sizeof(ProxyShape))) ProxyShape(this, newCollisionShape,
sizeof(ProxyShape))) ProxyShape(this, collisionShape,
transform, decimal(1));
// Add it to the list of proxy collision shapes of the body
@ -88,7 +85,7 @@ ProxyShape* CollisionBody::addCollisionShape(const CollisionShape& collisionShap
// Compute the world-space AABB of the new collision shape
AABB aabb;
newCollisionShape->computeAABB(aabb, mTransform * transform);
collisionShape->computeAABB(aabb, mTransform * transform);
// Notify the collision detection about this new collision shape
mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb);
@ -118,7 +115,6 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
mWorld.mCollisionDetection.removeProxyCollisionShape(current);
}
mWorld.removeCollisionShape(proxyShape->mCollisionShape);
current->~ProxyShape();
mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape));
mNbCollisionShapes--;
@ -140,7 +136,6 @@ void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove);
}
mWorld.removeCollisionShape(proxyShape->mCollisionShape);
elementToRemove->~ProxyShape();
mWorld.mMemoryAllocator.release(elementToRemove, sizeof(ProxyShape));
mNbCollisionShapes--;
@ -169,7 +164,6 @@ void CollisionBody::removeAllCollisionShapes() {
mWorld.mCollisionDetection.removeProxyCollisionShape(current);
}
mWorld.removeCollisionShape(current->mCollisionShape);
current->~ProxyShape();
mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape));
@ -203,15 +197,22 @@ void CollisionBody::updateBroadPhaseState() const {
// For all the proxy collision shapes of the body
for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) {
// Recompute the world-space AABB of the collision shape
AABB aabb;
shape->getCollisionShape()->computeAABB(aabb, mTransform *shape->getLocalToBodyTransform());
// Update the broad-phase state for the proxy collision shape
mWorld.mCollisionDetection.updateProxyCollisionShape(shape, aabb);
// Update the proxy
updateProxyShapeInBroadPhase(shape);
}
}
// 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());
// 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
/**
* @param isActive True if you want to activate the body

7
src/body/CollisionBody.h
View File

@ -102,6 +102,9 @@ class CollisionBody : public Body {
/// Update the broad-phase state for this body (because it has moved for instance)
virtual void updateBroadPhaseState() const;
/// Update the broad-phase state of a proxy collision shape of the body
void updateProxyShapeInBroadPhase(ProxyShape* proxyShape, bool forceReinsert = false) const;
/// Ask the broad-phase to test again the collision shapes of the body for collision
/// (as if the body has moved).
void askForBroadPhaseCollisionCheck() const;
@ -135,8 +138,8 @@ class CollisionBody : public Body {
virtual void setTransform(const Transform& transform);
/// Add a collision shape to the body.
virtual ProxyShape* addCollisionShape(const CollisionShape& collisionShape,
const Transform& transform);
virtual ProxyShape* addCollisionShape(CollisionShape* collisionShape,
const Transform& transform);
/// Remove a collision shape from the body
virtual void removeCollisionShape(const ProxyShape* proxyShape);

9
src/body/RigidBody.cpp
View File

@ -209,18 +209,15 @@ void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, cons
* @return A pointer to the proxy shape that has been created to link the body to
* the new collision shape you have added.
*/
ProxyShape* RigidBody::addCollisionShape(const CollisionShape& collisionShape,
ProxyShape* RigidBody::addCollisionShape(CollisionShape* collisionShape,
const Transform& transform,
decimal mass) {
assert(mass > decimal(0.0));
// Create an internal copy of the collision shape into the world if it is not there yet
CollisionShape* newCollisionShape = mWorld.createCollisionShape(collisionShape);
// Create a new proxy collision shape to attach the collision shape to the body
ProxyShape* proxyShape = new (mWorld.mMemoryAllocator.allocate(
sizeof(ProxyShape))) ProxyShape(this, newCollisionShape,
sizeof(ProxyShape))) ProxyShape(this, collisionShape,
transform, mass);
// Add it to the list of proxy collision shapes of the body
@ -234,7 +231,7 @@ ProxyShape* RigidBody::addCollisionShape(const CollisionShape& collisionShape,
// Compute the world-space AABB of the new collision shape
AABB aabb;
newCollisionShape->computeAABB(aabb, mTransform * transform);
collisionShape->computeAABB(aabb, mTransform * transform);
// Notify the collision detection about this new collision shape
mWorld.mCollisionDetection.addProxyCollisionShape(proxyShape, aabb);

2
src/body/RigidBody.h
View File

@ -210,7 +210,7 @@ class RigidBody : public CollisionBody {
void applyTorque(const Vector3& torque);
/// Add a collision shape to the body.
virtual ProxyShape* addCollisionShape(const CollisionShape& collisionShape,
virtual ProxyShape* addCollisionShape(CollisionShape* collisionShape,
const Transform& transform,
decimal mass);

255
src/collision/CollisionDetection.cpp
View File

@ -42,11 +42,15 @@ using namespace std;
// Constructor
CollisionDetection::CollisionDetection(CollisionWorld* world, MemoryAllocator& memoryAllocator)
: mWorld(world), mBroadPhaseAlgorithm(*this),
mNarrowPhaseGJKAlgorithm(memoryAllocator),
mNarrowPhaseSphereVsSphereAlgorithm(memoryAllocator),
: mMemoryAllocator(memoryAllocator),
mWorld(world), mBroadPhaseAlgorithm(*this),
mIsCollisionShapesAdded(false) {
// Set the default collision dispatch configuration
setCollisionDispatch(&mDefaultCollisionDispatch);
// Fill-in the collision detection matrix with algorithms
fillInCollisionMatrix();
}
// Destructor
@ -115,26 +119,29 @@ void CollisionDetection::reportCollisionBetweenShapes(CollisionCallback* callbac
continue;
}
// For each contact manifold of the overlapping pair
ContactManifold* manifold = pair->getContactManifold();
for (uint i=0; i<manifold->getNbContactPoints(); i++) {
// For each contact manifold set of the overlapping pair
const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
for (uint j=0; j<manifoldSet.getNbContactManifolds(); j++) {
ContactPoint* contactPoint = manifold->getContactPoint(i);
const ContactManifold* manifold = manifoldSet.getContactManifold(j);
// Create the contact info object for the contact
ContactPointInfo* contactInfo = new (mWorld->mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(manifold->getShape1(), manifold->getShape2(),
contactPoint->getNormal(),
contactPoint->getPenetrationDepth(),
contactPoint->getLocalPointOnBody1(),
contactPoint->getLocalPointOnBody2());
// For each contact manifold of the manifold set
for (uint i=0; i<manifold->getNbContactPoints(); i++) {
// Notify the collision callback about this new contact
if (callback != NULL) callback->notifyContact(*contactInfo);
ContactPoint* contactPoint = manifold->getContactPoint(i);
// Delete and remove the contact info from the memory allocator
contactInfo->~ContactPointInfo();
mWorld->mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo));
// Create the contact info object for the contact
ContactPointInfo contactInfo(manifold->getShape1(), manifold->getShape2(),
manifold->getShape1()->getCollisionShape(),
manifold->getShape2()->getCollisionShape(),
contactPoint->getNormal(),
contactPoint->getPenetrationDepth(),
contactPoint->getLocalPointOnBody1(),
contactPoint->getLocalPointOnBody2());
// Notify the collision callback about this new contact
if (callback != NULL) callback->notifyContact(contactInfo);
}
}
}
}
@ -158,11 +165,13 @@ void CollisionDetection::computeBroadPhase() {
void CollisionDetection::computeNarrowPhase() {
PROFILE("CollisionDetection::computeNarrowPhase()");
// Clear the set of overlapping pairs in narrow-phase contact
mContactOverlappingPairs.clear();
// For each possible collision pair of bodies
map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
ContactPointInfo* contactInfo = NULL;
OverlappingPair* pair = it->second;
@ -209,36 +218,30 @@ void CollisionDetection::computeNarrowPhase() {
if (mNoCollisionPairs.count(bodiesIndex) > 0) continue;
// Select the narrow phase algorithm to use according to the two collision shapes
NarrowPhaseAlgorithm& narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(
shape1->getCollisionShape(),
shape2->getCollisionShape());
const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Type][shape2Type];
// If there is no collision algorithm between those two kinds of shapes
if (narrowPhaseAlgorithm == NULL) continue;
// Notify the narrow-phase algorithm about the overlapping pair we are going to test
narrowPhaseAlgorithm.setCurrentOverlappingPair(pair);
narrowPhaseAlgorithm->setCurrentOverlappingPair(pair);
// Create the CollisionShapeInfo objects
CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(),
pair, shape1->getCachedCollisionData());
CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(),
pair, shape2->getCachedCollisionData());
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision
if (narrowPhaseAlgorithm.testCollision(shape1, shape2, contactInfo)) {
assert(contactInfo != NULL);
// If it is the first contact since the pair are overlapping
if (pair->getNbContactPoints() == 0) {
// Trigger a callback event
if (mWorld->mEventListener != NULL) mWorld->mEventListener->beginContact(*contactInfo);
}
// Create a new contact
createContact(pair, contactInfo);
// Trigger a callback event for the new contact
if (mWorld->mEventListener != NULL) mWorld->mEventListener->newContact(*contactInfo);
// Delete and remove the contact info from the memory allocator
contactInfo->~ContactPointInfo();
mWorld->mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo));
}
// if there really is a collision. If a collision occurs, the
// notifyContact() callback method will be called.
narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, this);
}
// Add all the contact manifolds (between colliding bodies) to the bodies
addAllContactManifoldsToBodies();
}
// Compute the narrow-phase collision detection
@ -246,10 +249,11 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call
const std::set<uint>& shapes1,
const std::set<uint>& shapes2) {
mContactOverlappingPairs.clear();
// For each possible collision pair of bodies
map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
ContactPointInfo* contactInfo = NULL;
OverlappingPair* pair = it->second;
@ -316,29 +320,31 @@ void CollisionDetection::computeNarrowPhaseBetweenShapes(CollisionCallback* call
if (body1->isSleeping() && body2->isSleeping()) continue;
// Select the narrow phase algorithm to use according to the two collision shapes
NarrowPhaseAlgorithm& narrowPhaseAlgorithm = selectNarrowPhaseAlgorithm(
shape1->getCollisionShape(),
shape2->getCollisionShape());
const CollisionShapeType shape1Type = shape1->getCollisionShape()->getType();
const CollisionShapeType shape2Type = shape2->getCollisionShape()->getType();
NarrowPhaseAlgorithm* narrowPhaseAlgorithm = mCollisionMatrix[shape1Type][shape2Type];
// If there is no collision algorithm between those two kinds of shapes
if (narrowPhaseAlgorithm == NULL) continue;
// Notify the narrow-phase algorithm about the overlapping pair we are going to test
narrowPhaseAlgorithm.setCurrentOverlappingPair(pair);
narrowPhaseAlgorithm->setCurrentOverlappingPair(pair);
// Create the CollisionShapeInfo objects
CollisionShapeInfo shape1Info(shape1, shape1->getCollisionShape(), shape1->getLocalToWorldTransform(),
pair, shape1->getCachedCollisionData());
CollisionShapeInfo shape2Info(shape2, shape2->getCollisionShape(), shape2->getLocalToWorldTransform(),
pair, shape2->getCachedCollisionData());
TestCollisionBetweenShapesCallback narrowPhaseCallback(callback);
// Use the narrow-phase collision detection algorithm to check
// if there really is a collision
if (narrowPhaseAlgorithm.testCollision(shape1, shape2, contactInfo)) {
assert(contactInfo != NULL);
// Create a new contact
createContact(pair, contactInfo);
// Notify the collision callback about this new contact
if (callback != NULL) callback->notifyContact(*contactInfo);
// Delete and remove the contact info from the memory allocator
contactInfo->~ContactPointInfo();
mWorld->mMemoryAllocator.release(contactInfo, sizeof(ContactPointInfo));
}
narrowPhaseAlgorithm->testCollision(shape1Info, shape2Info, &narrowPhaseCallback);
}
// Add all the contact manifolds (between colliding bodies) to the bodies
addAllContactManifoldsToBodies();
}
// Allow the broadphase to notify the collision detection about an overlapping pair.
@ -360,9 +366,13 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
// Check if the overlapping pair already exists
if (mOverlappingPairs.find(pairID) != mOverlappingPairs.end()) return;
// Compute the maximum number of contact manifolds for this pair
int nbMaxManifolds = CollisionShape::computeNbMaxContactManifolds(shape1->getCollisionShape()->getType(),
shape2->getCollisionShape()->getType());
// Create the overlapping pair and add it into the set of overlapping pairs
OverlappingPair* newPair = new (mWorld->mMemoryAllocator.allocate(sizeof(OverlappingPair)))
OverlappingPair(shape1, shape2, mWorld->mMemoryAllocator);
OverlappingPair(shape1, shape2, nbMaxManifolds, mWorld->mMemoryAllocator);
assert(newPair != NULL);
std::pair<map<overlappingpairid, OverlappingPair*>::iterator, bool> check =
mOverlappingPairs.insert(make_pair(pairID, newPair));
@ -400,47 +410,85 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
mBroadPhaseAlgorithm.removeProxyCollisionShape(proxyShape);
}
// Called by a narrow-phase collision algorithm when a new contact has been found
void CollisionDetection::notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo) {
// If it is the first contact since the pairs are overlapping
if (overlappingPair->getNbContactPoints() == 0) {
// Trigger a callback event
if (mWorld->mEventListener != NULL) mWorld->mEventListener->beginContact(contactInfo);
}
// Create a new contact
createContact(overlappingPair, contactInfo);
// Trigger a callback event for the new contact
if (mWorld->mEventListener != NULL) mWorld->mEventListener->newContact(contactInfo);
}
// Create a new contact
void CollisionDetection::createContact(OverlappingPair* overlappingPair,
const ContactPointInfo* contactInfo) {
const ContactPointInfo& contactInfo) {
// Create a new contact
ContactPoint* contact = new (mWorld->mMemoryAllocator.allocate(sizeof(ContactPoint)))
ContactPoint(*contactInfo);
assert(contact != NULL);
ContactPoint(contactInfo);
// Add the contact to the contact cache of the corresponding overlapping pair
// Add the contact to the contact manifold set of the corresponding overlapping pair
overlappingPair->addContact(contact);
// Add the contact manifold into the list of contact manifolds
// of the two bodies involved in the contact
addContactManifoldToBody(overlappingPair->getContactManifold(),
overlappingPair->getShape1()->getBody(),
overlappingPair->getShape2()->getBody());
// Add the overlapping pair into the set of pairs in contact during narrow-phase
overlappingpairid pairId = OverlappingPair::computeID(overlappingPair->getShape1(),
overlappingPair->getShape2());
mContactOverlappingPairs[pairId] = overlappingPair;
}
void CollisionDetection::addAllContactManifoldsToBodies() {
// For each overlapping pairs in contact during the narrow-phase
std::map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mContactOverlappingPairs.begin(); it != mContactOverlappingPairs.end(); ++it) {
// Add all the contact manifolds of the pair into the list of contact manifolds
// of the two bodies involved in the contact
addContactManifoldToBody(it->second);
}
}
// Add a contact manifold to the linked list of contact manifolds of the two bodies involved
// in the corresponding contact
void CollisionDetection::addContactManifoldToBody(ContactManifold* contactManifold,
CollisionBody* body1, CollisionBody* body2) {
void CollisionDetection::addContactManifoldToBody(OverlappingPair* pair) {
assert(contactManifold != NULL);
assert(pair != NULL);
// Add the contact manifold at the beginning of the linked
// list of contact manifolds of the first body
void* allocatedMemory1 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement1 = new (allocatedMemory1)
ContactManifoldListElement(contactManifold,
body1->mContactManifoldsList);
body1->mContactManifoldsList = listElement1;
CollisionBody* body1 = pair->getShape1()->getBody();
CollisionBody* body2 = pair->getShape2()->getBody();
const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
// Add the contact manifold at the beginning of the linked
// list of the contact manifolds of the second body
void* allocatedMemory2 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement2 = new (allocatedMemory2)
ContactManifoldListElement(contactManifold,
body2->mContactManifoldsList);
body2->mContactManifoldsList = listElement2;
// For each contact manifold in the set of manifolds in the pair
for (int i=0; i<manifoldSet.getNbContactManifolds(); i++) {
ContactManifold* contactManifold = manifoldSet.getContactManifold(i);
assert(contactManifold->getNbContactPoints() > 0);
// Add the contact manifold at the beginning of the linked
// list of contact manifolds of the first body
void* allocatedMemory1 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement1 = new (allocatedMemory1)
ContactManifoldListElement(contactManifold,
body1->mContactManifoldsList);
body1->mContactManifoldsList = listElement1;
// Add the contact manifold at the beginning of the linked
// list of the contact manifolds of the second body
void* allocatedMemory2 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement2 = new (allocatedMemory2)
ContactManifoldListElement(contactManifold,
body2->mContactManifoldsList);
body2->mContactManifoldsList = listElement2;
}
}
// Delete all the contact points in the currently overlapping pairs
@ -452,3 +500,30 @@ void CollisionDetection::clearContactPoints() {
it->second->clearContactPoints();
}
}
// Fill-in the collision detection matrix
void CollisionDetection::fillInCollisionMatrix() {
// For each possible type of collision shape
for (int i=0; i<NB_COLLISION_SHAPE_TYPES; i++) {
for (int j=0; j<NB_COLLISION_SHAPE_TYPES; j++) {
mCollisionMatrix[i][j] = mCollisionDispatch->selectAlgorithm(i, j);
}
}
}
// Return the world event listener
EventListener* CollisionDetection::getWorldEventListener() {
return mWorld->mEventListener;
}
/// Return a reference to the world memory allocator
MemoryAllocator& CollisionDetection::getWorldMemoryAllocator() {
return mWorld->mMemoryAllocator;
}
// Called by a narrow-phase collision algorithm when a new contact has been found
void TestCollisionBetweenShapesCallback::notifyContact(OverlappingPair* overlappingPair,
const ContactPointInfo& contactInfo) {
mCollisionCallback->notifyContact(contactInfo);
}

122
src/collision/CollisionDetection.h
View File

@ -30,8 +30,8 @@
#include "body/CollisionBody.h"
#include "broadphase/BroadPhaseAlgorithm.h"
#include "engine/OverlappingPair.h"
#include "narrowphase/GJK/GJKAlgorithm.h"
#include "narrowphase/SphereVsSphereAlgorithm.h"
#include "engine/EventListener.h"
#include "narrowphase/DefaultCollisionDispatch.h"
#include "memory/MemoryAllocator.h"
#include "constraint/ContactPoint.h"
#include <vector>
@ -47,6 +47,26 @@ class BroadPhaseAlgorithm;
class CollisionWorld;
class CollisionCallback;
// Class TestCollisionBetweenShapesCallback
class TestCollisionBetweenShapesCallback : public NarrowPhaseCallback {
private:
CollisionCallback* mCollisionCallback;
public:
// Constructor
TestCollisionBetweenShapesCallback(CollisionCallback* callback)
: mCollisionCallback(callback) {
}
// Called by a narrow-phase collision algorithm when a new contact has been found
virtual void notifyContact(OverlappingPair* overlappingPair,
const ContactPointInfo& contactInfo);
};
// Class CollisionDetection
/**
* This class computes the collision detection algorithms. We first
@ -54,25 +74,42 @@ class CollisionCallback;
* collide and then we run a narrow-phase algorithm to compute the
* collision contacts between bodies.
*/
class CollisionDetection {
class CollisionDetection : public NarrowPhaseCallback {
private :
// -------------------- Attributes -------------------- //
/// Collision Detection Dispatch configuration
CollisionDispatch* mCollisionDispatch;
/// Default collision dispatch configuration
DefaultCollisionDispatch mDefaultCollisionDispatch;
/// 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;
/// 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;
/// Narrow-phase GJK algorithm
// TODO : Delete this
GJKAlgorithm mNarrowPhaseGJKAlgorithm;
/// Narrow-phase Sphere vs Sphere algorithm
// TODO : Delete this
SphereVsSphereAlgorithm mNarrowPhaseSphereVsSphereAlgorithm;
/// Set of pair of bodies that cannot collide between each other
@ -95,20 +132,18 @@ class CollisionDetection {
/// Compute the narrow-phase collision detection
void computeNarrowPhase();
/// Select the narrow phase algorithm to use given two collision shapes
NarrowPhaseAlgorithm& selectNarrowPhaseAlgorithm(const CollisionShape* collisionShape1,
const CollisionShape* collisionShape2);
/// Create a new contact
void createContact(OverlappingPair* overlappingPair, const ContactPointInfo* contactInfo);
/// Add a contact manifold to the linked list of contact manifolds of the two bodies
/// involed in the corresponding contact.
void addContactManifoldToBody(ContactManifold* contactManifold,
CollisionBody *body1, CollisionBody *body2);
void addContactManifoldToBody(OverlappingPair* pair);
/// Delete all the contact points in the currently overlapping pairs
void clearContactPoints();
/// Fill-in the collision detection matrix
void fillInCollisionMatrix();
/// Add all the contact manifold of colliding pairs to their bodies
void addAllContactManifoldsToBodies();
public :
@ -120,6 +155,13 @@ class CollisionDetection {
/// Destructor
~CollisionDetection();
/// 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);
@ -128,7 +170,7 @@ class CollisionDetection {
/// Update a proxy collision shape (that has moved for instance)
void updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb,
const Vector3& displacement = Vector3(0, 0, 0));
const Vector3& displacement = Vector3(0, 0, 0), bool forceReinsert = false);
/// Add a pair of bodies that cannot collide with each other
void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2);
@ -167,30 +209,47 @@ class CollisionDetection {
/// Allow the broadphase to notify the collision detection about an overlapping pair.
void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
// Compute the narrow-phase collision detection
/// 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();
/// Called by a narrow-phase collision algorithm when a new contact has been found
virtual void notifyContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo);
/// Create a new contact
void createContact(OverlappingPair* overlappingPair, const ContactPointInfo& contactInfo);
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
friend class ConvexMeshShape;
};
// Select the narrow-phase collision algorithm to use given two collision shapes
inline NarrowPhaseAlgorithm& CollisionDetection::selectNarrowPhaseAlgorithm(
const CollisionShape* collisionShape1,
const CollisionShape* collisionShape2) {
// Sphere vs Sphere algorithm
if (collisionShape1->getType() == SPHERE && collisionShape2->getType() == SPHERE) {
return mNarrowPhaseSphereVsSphereAlgorithm;
}
else { // GJK algorithm
return mNarrowPhaseGJKAlgorithm;
}
}
// 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[shape1Type][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();
}
// Add a body to the collision detection
inline void CollisionDetection::addProxyCollisionShape(ProxyShape* proxyShape,
@ -223,7 +282,7 @@ inline void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape
// Update a proxy collision shape (that has moved for instance)
inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb,
const Vector3& displacement) {
const Vector3& displacement, bool forceReinsert) {
mBroadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement);
}
@ -232,6 +291,8 @@ inline void CollisionDetection::raycast(RaycastCallback* raycastCallback,
const Ray& ray,
unsigned short raycastWithCategoryMaskBits) const {
PROFILE("CollisionDetection::raycast()");
RaycastTest rayCastTest(raycastCallback);
// Ask the broad-phase algorithm to call the testRaycastAgainstShape()
@ -251,6 +312,11 @@ inline bool CollisionDetection::testAABBOverlap(const ProxyShape* shape1,
return mBroadPhaseAlgorithm.testOverlappingShapes(shape1, shape2);
}
// Return a pointer to the world
inline CollisionWorld* CollisionDetection::getWorld() {
return mWorld;
}
}
#endif

75
src/collision/CollisionShapeInfo.h Normal file
View File

@ -0,0 +1,75 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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

15
src/engine/ContactManifold.cpp → src/collision/ContactManifold.cpp
View File

@ -31,9 +31,10 @@ using namespace reactphysics3d;
// Constructor
ContactManifold::ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator)
: mShape1(shape1), mShape2(shape2), mNbContactPoints(0), mFrictionImpulse1(0.0),
mFrictionImpulse2(0.0), mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
MemoryAllocator& memoryAllocator, short normalDirectionId)
: mShape1(shape1), mShape2(shape2), mNormalDirectionId(normalDirectionId),
mNbContactPoints(0), mFrictionImpulse1(0.0), mFrictionImpulse2(0.0),
mFrictionTwistImpulse(0.0), mIsAlreadyInIsland(false),
mMemoryAllocator(memoryAllocator) {
}
@ -58,10 +59,10 @@ void ContactManifold::addContactPoint(ContactPoint* contact) {
// Delete the new contact
contact->~ContactPoint();
mMemoryAllocator.release(contact, sizeof(ContactPoint));
//removeContact(i);
assert(mNbContactPoints > 0);
return;
//break;
}
}
@ -75,6 +76,8 @@ void ContactManifold::addContactPoint(ContactPoint* contact) {
// Add the new contact point in the manifold
mContactPoints[mNbContactPoints] = contact;
mNbContactPoints++;
assert(mNbContactPoints > 0);
}
// Remove a contact point from the manifold
@ -93,6 +96,8 @@ void ContactManifold::removeContactPoint(uint index) {
}
mNbContactPoints--;
assert(mNbContactPoints >= 0);
}
// Update the contact manifold

44
src/engine/ContactManifold.h → src/collision/ContactManifold.h
View File

@ -98,6 +98,9 @@ class ContactManifold {
/// Contact points in the manifold
ContactPoint* mContactPoints[MAX_CONTACT_POINTS_IN_MANIFOLD];
/// Normal direction Id (Unique Id representing the normal direction)
short int mNormalDirectionId;
/// Number of contacts in the cache
uint mNbContactPoints;
@ -151,7 +154,7 @@ class ContactManifold {
/// Constructor
ContactManifold(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator);
MemoryAllocator& memoryAllocator, short int normalDirectionId);
/// Destructor
~ContactManifold();
@ -168,6 +171,9 @@ 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);
@ -213,6 +219,12 @@ class ContactManifold {
/// 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;
// -------------------- Friendship -------------------- //
friend class DynamicsWorld;
@ -240,6 +252,11 @@ 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 {
return mNbContactPoints;
@ -306,6 +323,31 @@ inline bool ContactManifold::isAlreadyInIsland() const {
return mIsAlreadyInIsland;
}
// Return the normalized averaged normal vector
inline Vector3 ContactManifold::getAverageContactNormal() const {
Vector3 averageNormal;
for (int 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 (int i=0; i<mNbContactPoints; i++) {
decimal depth = mContactPoints[i]->getPenetrationDepth();
if (depth > largestDepth) {
largestDepth = depth;
}
}
return largestDepth;
}
}
#endif

236
src/collision/ContactManifoldSet.cpp Normal file
View File

@ -0,0 +1,236 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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 "ContactManifoldSet.h"
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);
}
// Destructor
ContactManifoldSet::~ContactManifoldSet() {
// Clear all the contact manifolds
clear();
}
// Add a contact point to the manifold set
void ContactManifoldSet::addContactPoint(ContactPoint* contact) {
// Compute an Id corresponding to the normal direction (using a cubemap)
short int normalDirectionId = computeCubemapNormalId(contact->getNormal());
// 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);
}
// If a similar manifold has been found
if (similarManifoldIndex != -1) {
// 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;
}
else {
faceNo = normalScaled.z > 0 ? 4 : 5;
u = normalScaled.x;
v = normalScaled.y;
}
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() {
for (int i=mNbManifolds-1; i>=0; i--) {
// Update the contact manifold
mManifolds[i]->update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(),
mShape2->getBody()->getTransform() * mShape2->getLocalToBodyTransform());
// Remove the contact manifold if has no contact points anymore
if (mManifolds[i]->getNbContactPoints() == 0) {
removeManifold(i);
}
}
}
// Clear the contact manifold set
void ContactManifoldSet::clear() {
// Destroy all the contact manifolds
for (int i=mNbManifolds-1; i>=0; i--) {
removeManifold(i);
}
assert(mNbManifolds == 0);
}
// Create a new contact manifold and add it to the set
void ContactManifoldSet::createManifold(short int normalDirectionId) {
assert(mNbManifolds < mNbMaxManifolds);
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) {
assert(mNbManifolds > 0);
assert(index >= 0 && index < mNbManifolds);
// Delete the new contact
mManifolds[index]->~ContactManifold();
mMemoryAllocator.release(mManifolds[index], sizeof(ContactManifold));
for (int i=index; (i+1) < mNbManifolds; i++) {
mManifolds[i] = mManifolds[i+1];
}
mNbManifolds--;
}

154
src/collision/ContactManifoldSet.h Normal file
View File

@ -0,0 +1,154 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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_CONTACT_MANIFOLD_SET_H
#define REACTPHYSICS3D_CONTACT_MANIFOLD_SET_H
// Libraries
#include "ContactManifold.h"
namespace reactphysics3d {
// Constants
const int MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET = 3; // Maximum number of contact manifolds in the set
const int CONTACT_CUBEMAP_FACE_NB_SUBDIVISIONS = 3; // N Number for the N x N subdivisions of the cubemap
// Class ContactManifoldSet
/**
* This class represents a set of one or several contact manifolds. Typically a
* convex/convex collision will have a set with a single manifold and a convex-concave
* collision can have more than one manifolds. Note that a contact manifold can
* contains several contact points.
*/
class ContactManifoldSet {
private:
// -------------------- Attributes -------------------- //
/// Maximum number of contact manifolds in the set
int mNbMaxManifolds;
/// Current number of contact manifolds in the set
int mNbManifolds;
/// Pointer to the first proxy shape of the contact
ProxyShape* mShape1;
/// Pointer to the second proxy shape of the contact
ProxyShape* mShape2;
/// Reference to the memory allocator
MemoryAllocator& mMemoryAllocator;
/// Contact manifolds of the set
ContactManifold* mManifolds[MAX_MANIFOLDS_IN_CONTACT_MANIFOLD_SET];
// -------------------- Methods -------------------- //
/// Create a new contact manifold and add it to the set
void createManifold(short normalDirectionId);
/// Remove a contact manifold from the set
void removeManifold(int index);
// Return the index of the contact manifold with a similar average normal.
int selectManifoldWithSimilarNormal(short int normalDirectionId) const;
// 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;
public:
// -------------------- Methods -------------------- //
/// Constructor
ContactManifoldSet(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator, int nbMaxManifolds);
/// Destructor
~ContactManifoldSet();
/// 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(uint index) const;
/// Return the total number of contact points in the set of manifolds
int getTotalNbContactPoints() const;
};
// Return the first proxy shape
inline ProxyShape* ContactManifoldSet::getShape1() const {
return mShape1;
}
// Return the second proxy shape
inline ProxyShape* ContactManifoldSet::getShape2() const {
return mShape2;
}
// Return the number of manifolds in the set
inline int ContactManifoldSet::getNbContactManifolds() const {
return mNbManifolds;
}
// Return a given contact manifold
inline ContactManifold* ContactManifoldSet::getContactManifold(uint index) const {
assert(index >= 0 && index < mNbManifolds);
return mManifolds[index];
}
// 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();
}
return nbPoints;
}
}
#endif

31
src/collision/ProxyShape.cpp
View File

@ -35,8 +35,7 @@ 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)
ProxyShape::ProxyShape(CollisionBody* body, CollisionShape* shape, const Transform& transform, decimal mass)
:mBody(body), mCollisionShape(shape), mLocalToBodyTransform(transform), mMass(mass),
mNext(NULL), mBroadPhaseID(-1), mCachedCollisionData(NULL), mUserData(NULL),
mCollisionCategoryBits(0x0001), mCollideWithMaskBits(0xFFFF) {
@ -63,3 +62,31 @@ bool ProxyShape::testPointInside(const Vector3& worldPoint) {
return mCollisionShape->testPointInside(localPoint, this);
}
// Raycast method with feedback information
/**
* @param ray Ray to use for the raycasting
* @param[out] raycastInfo Result of the raycasting that is valid only if the
* methods returned true
* @return True if the ray hit the collision shape
*/
bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
// If the corresponding body is not active, it cannot be hit by rays
if (!mBody->isActive()) return false;
// Convert the ray into the local-space of the collision shape
const Transform localToWorldTransform = getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
Ray rayLocal(worldToLocalTransform * ray.point1,
worldToLocalTransform * ray.point2,
ray.maxFraction);
bool isHit = mCollisionShape->raycast(rayLocal, raycastInfo, this);
// Convert the raycast info into world-space
raycastInfo.worldPoint = localToWorldTransform * raycastInfo.worldPoint;
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * raycastInfo.worldNormal;
raycastInfo.worldNormal.normalize();
return isHit;
}

95
src/collision/ProxyShape.h
View File

@ -55,7 +55,7 @@ class ProxyShape {
CollisionShape* mCollisionShape;
/// Local-space to parent body-space transform (does not change over time)
const Transform mLocalToBodyTransform;
Transform mLocalToBodyTransform;
/// Mass (in kilogramms) of the corresponding collision shape
decimal mMass;
@ -93,15 +93,6 @@ class ProxyShape {
/// Private assignment operator
ProxyShape& operator=(const ProxyShape& proxyShape);
// Return a local support point in a given direction with the object margin
Vector3 getLocalSupportPointWithMargin(const Vector3& direction);
/// Return a local support point in a given direction without the object margin.
Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction);
/// Return the collision shape margin
decimal getMargin() const;
public:
// -------------------- Methods -------------------- //
@ -131,6 +122,9 @@ class ProxyShape {
/// Return the local to parent body transform
const Transform& getLocalToBodyTransform() const;
/// Set the local to parent body transform
void setLocalToBodyTransform(const Transform& transform);
/// Return the local to world transform
const Transform getLocalToWorldTransform() const;
@ -158,6 +152,15 @@ 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);
// -------------------- Friendship -------------------- //
friend class OverlappingPair;
@ -171,9 +174,15 @@ class ProxyShape {
friend class EPAAlgorithm;
friend class GJKAlgorithm;
friend class ConvexMeshShape;
};
/// Return the collision shape
// 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
*/
@ -222,6 +231,17 @@ inline const Transform& ProxyShape::getLocalToBodyTransform() const {
return mLocalToBodyTransform;
}
// Set the local to parent body transform
inline void ProxyShape::setLocalToBodyTransform(const Transform& transform) {
mLocalToBodyTransform = transform;
mBody->setIsSleeping(false);
// Notify the body that the proxy shape has to be updated in the broad-phase
mBody->updateProxyShapeInBroadPhase(this, true);
}
// Return the local to world transform
/**
* @return The transformation that transforms the local-space of the collision
@ -231,36 +251,6 @@ inline const Transform ProxyShape::getLocalToWorldTransform() const {
return mBody->mTransform * mLocalToBodyTransform;
}
// Return a local support point in a given direction with the object margin
inline Vector3 ProxyShape::getLocalSupportPointWithMargin(const Vector3& direction) {
return mCollisionShape->getLocalSupportPointWithMargin(direction, &mCachedCollisionData);
}
// Return a local support point in a given direction without the object margin.
inline Vector3 ProxyShape::getLocalSupportPointWithoutMargin(const Vector3& direction) {
return mCollisionShape->getLocalSupportPointWithoutMargin(direction, &mCachedCollisionData);
}
// Return the collision shape margin
inline decimal ProxyShape::getMargin() const {
return mCollisionShape->getMargin();
}
// Raycast method with feedback information
/**
* @param ray Ray to use for the raycasting
* @param[out] raycastInfo Result of the raycasting that is valid only if the
* methods returned true
* @return True if the ray hit the collision shape
*/
inline bool ProxyShape::raycast(const Ray& ray, RaycastInfo& raycastInfo) {
// If the corresponding body is not active, it cannot be hit by rays
if (!mBody->isActive()) return false;
return mCollisionShape->raycast(ray, raycastInfo, this);
}
// 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
@ -309,6 +299,29 @@ inline void ProxyShape::setCollideWithMaskBits(unsigned short collideWithMaskBit
mCollideWithMaskBits = collideWithMaskBits;
}
// Return the local scaling vector of the collision shape
/**
* @return The local scaling vector
*/
inline Vector3 ProxyShape::getLocalScaling() const {
return mCollisionShape->getScaling();
}
// Set the local scaling vector of the collision shape
/**
* @param scaling The new local scaling vector
*/
inline void ProxyShape::setLocalScaling(const Vector3& scaling) {
// Set the local scaling of the collision shape
mCollisionShape->setLocalScaling(scaling);
mBody->setIsSleeping(false);
// Notify the body that the proxy shape has to be updated in the broad-phase
mBody->updateProxyShapeInBroadPhase(this, true);
}
}
#endif

10
src/collision/RaycastInfo.h
View File

@ -68,6 +68,12 @@ struct RaycastInfo {
/// The hit point "p" is such that p = point1 + hitFraction * (point2 - point1)
decimal hitFraction;
/// Mesh subpart index that has been hit (only used for triangles mesh and -1 otherwise)
int meshSubpart;
/// Hit triangle index (only used for triangles mesh and -1 otherwise)
int triangleIndex;
/// Pointer to the hit collision body
CollisionBody* body;
@ -77,7 +83,7 @@ struct RaycastInfo {
// -------------------- Methods -------------------- //
/// Constructor
RaycastInfo() : body(NULL), proxyShape(NULL) {
RaycastInfo() : meshSubpart(-1), triangleIndex(-1), body(NULL), proxyShape(NULL) {
}
@ -108,7 +114,7 @@ class RaycastCallback {
/// This method will be called for each ProxyShape that is hit by the
/// ray. You cannot make any assumptions about the order of the
/// calls. You should use the return value to control the continuation
/// of the ray. The return value is the next maxFraction value to use.
/// of the ray. The returned value is the next maxFraction value to use.
/// If you return a fraction of 0.0, it means that the raycast should
/// terminate. If you return a fraction of 1.0, it indicates that the
/// ray is not clipped and the ray cast should continue as if no hit

39
src/collision/TriangleMesh.cpp Normal file
View File

@ -0,0 +1,39 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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 "TriangleMesh.h"
using namespace reactphysics3d;
// Constructor
TriangleMesh::TriangleMesh() {
}
// Destructor
TriangleMesh::~TriangleMesh() {
}

88
src/collision/TriangleMesh.h Normal file
View File

@ -0,0 +1,88 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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_MESH_H
#define REACTPHYSICS3D_TRIANGLE_MESH_H
// Libraries
#include <vector>
#include <cassert>
#include "TriangleVertexArray.h"
namespace reactphysics3d {
// Class TriangleMesh
/**
* 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
* mesh for instance.
*/
class TriangleMesh {
protected:
/// All the triangle arrays of the mesh (one triangle array per part)
std::vector<TriangleVertexArray*> mTriangleArrays;
public:
/// Constructor
TriangleMesh();
/// Destructor
~TriangleMesh();
/// Add a subpart of the mesh
void addSubpart(TriangleVertexArray* triangleVertexArray);
/// Return a pointer to a given subpart (triangle vertex array) of the mesh
TriangleVertexArray* getSubpart(uint indexSubpart) const;
/// Return the number of subparts of the mesh
uint getNbSubparts() const;
};
// Add a subpart of the mesh
inline void TriangleMesh::addSubpart(TriangleVertexArray* triangleVertexArray) {
mTriangleArrays.push_back(triangleVertexArray );
}
// Return a pointer to a given subpart (triangle vertex array) of the mesh
inline TriangleVertexArray* TriangleMesh::getSubpart(uint indexSubpart) const {
assert(indexSubpart < mTriangleArrays.size());
return mTriangleArrays[indexSubpart];
}
// Return the number of subparts of the mesh
inline uint TriangleMesh::getNbSubparts() const {
return mTriangleArrays.size();
}
}
#endif

48
src/collision/TriangleVertexArray.cpp Normal file
View File

@ -0,0 +1,48 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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 "TriangleVertexArray.h"
using namespace reactphysics3d;
// Constructor
TriangleVertexArray::TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
uint nbTriangles, void* indexesStart, int indexesStride,
VertexDataType vertexDataType, IndexDataType indexDataType) {
mNbVertices = nbVertices;
mVerticesStart = reinterpret_cast<unsigned char*>(verticesStart);
mVerticesStride = verticesStride;
mNbTriangles = nbTriangles;
mIndicesStart = reinterpret_cast<unsigned char*>(indexesStart);
mIndicesStride = indexesStride;
mVertexDataType = vertexDataType;
mIndexDataType = indexDataType;
}
// Destructor
TriangleVertexArray::~TriangleVertexArray() {
}

160
src/collision/TriangleVertexArray.h Normal file
View File

@ -0,0 +1,160 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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_VERTEX_ARRAY_H
#define REACTPHYSICS3D_TRIANGLE_VERTEX_ARRAY_H
// Libraries
#include "configuration.h"
namespace reactphysics3d {
// Class TriangleVertexArray
/**
* This class is used to describe the vertices and faces of a triangular mesh.
* A TriangleVertexArray represents a continuous array of vertices and indexes
* of a triangular mesh. When you create a TriangleVertexArray, 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 TriangleVertexArray
* remain valid during the TriangleVertexArray life.
*/
class TriangleVertexArray {
public:
/// Data type for the vertices in the array
enum VertexDataType {VERTEX_FLOAT_TYPE, VERTEX_DOUBLE_TYPE};
/// Data type for the indices in the array
enum IndexDataType {INDEX_INTEGER_TYPE, INDEX_SHORT_TYPE};
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;
/// Number of triangles in the array
uint mNbTriangles;
/// 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;
/// Data type of the vertices in the array
VertexDataType mVertexDataType;
/// Data type of the indices in the array
IndexDataType mIndexDataType;
public:
/// Constructor
TriangleVertexArray(uint nbVertices, void* verticesStart, int verticesStride,
uint nbTriangles, void* indexesStart, int indexesStride,
VertexDataType vertexDataType, IndexDataType indexDataType);
/// Destructor
~TriangleVertexArray();
/// 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 triangles
uint getNbTriangles() const;
/// Return the vertices stride (number of bytes)
int getVerticesStride() const;
/// Return the indices stride (number of bytes)
int getIndicesStride() 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 TriangleVertexArray::VertexDataType TriangleVertexArray::getVertexDataType() const {
return mVertexDataType;
}
// Return the index data type
inline TriangleVertexArray::IndexDataType TriangleVertexArray::getIndexDataType() const {
return mIndexDataType;
}
// Return the number of vertices
inline uint TriangleVertexArray::getNbVertices() const {
return mNbVertices;
}
// Return the number of triangles
inline uint TriangleVertexArray::getNbTriangles() const {
return mNbTriangles;
}
// Return the vertices stride (number of bytes)
inline int TriangleVertexArray::getVerticesStride() const {
return mVerticesStride;
}
// Return the indices stride (number of bytes)
inline int TriangleVertexArray::getIndicesStride() const {
return mIndicesStride;
}
// Return the pointer to the start of the vertices array
inline unsigned char* TriangleVertexArray::getVerticesStart() const {
return mVerticesStart;
}
// Return the pointer to the start of the indices array
inline unsigned char* TriangleVertexArray::getIndicesStart() const {
return mIndicesStart;
}
}
#endif

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

@ -33,7 +33,7 @@ using namespace reactphysics3d;
// Constructor
BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
:mDynamicAABBTree(*this), mNbMovedShapes(0), mNbAllocatedMovedShapes(8),
:mDynamicAABBTree(DYNAMIC_TREE_AABB_GAP), mNbMovedShapes(0), mNbAllocatedMovedShapes(8),
mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8),
mCollisionDetection(collisionDetection) {
@ -118,7 +118,10 @@ void BroadPhaseAlgorithm::removeMovedCollisionShape(int broadPhaseID) {
void BroadPhaseAlgorithm::addProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) {
// Add the collision shape into the dynamic AABB tree and get its broad-phase ID
mDynamicAABBTree.addObject(proxyShape, aabb);
int nodeId = mDynamicAABBTree.addObject(aabb, proxyShape);
// Set the broad-phase ID of the proxy shape
proxyShape->mBroadPhaseID = nodeId;
// Add the collision shape into the array of bodies that have moved (or have been created)
// during the last simulation step
@ -140,14 +143,14 @@ void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) {
// Notify the broad-phase that a collision shape has moved and need to be updated
void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb,
const Vector3& displacement) {
const Vector3& displacement, bool forceReinsert) {
int broadPhaseID = proxyShape->mBroadPhaseID;
assert(broadPhaseID >= 0);
// Update the dynamic AABB tree according to the movement of the collision shape
bool hasBeenReInserted = mDynamicAABBTree.updateObject(broadPhaseID, aabb, displacement);
bool hasBeenReInserted = mDynamicAABBTree.updateObject(broadPhaseID, aabb, displacement, forceReinsert);
// If the collision shape has moved out of its fat AABB (and therefore has been reinserted
// into the tree).
@ -172,13 +175,15 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
if (shapeID == -1) continue;
// Get the fat AABB of the collision shape from the dynamic AABB tree
AABBOverlapCallback callback(*this, shapeID);
// Get the AABB of the shape
const AABB& shapeAABB = mDynamicAABBTree.getFatAABB(shapeID);
// Ask the dynamic AABB tree to report all collision shapes that overlap with
// this AABB. The method BroadPhase::notifiyOverlappingPair() will be called
// by the dynamic AABB tree for each potential overlapping pair.
mDynamicAABBTree.reportAllShapesOverlappingWith(shapeID, shapeAABB);
mDynamicAABBTree.reportAllShapesOverlappingWithAABB(shapeAABB, callback);
}
// Reset the array of collision shapes that have move (or have been created) during the
@ -197,9 +202,11 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
BroadPhasePair* pair = mPotentialPairs + i;
i++;
assert(pair->collisionShape1ID != pair->collisionShape2ID);
// Get the two collision shapes of the pair
ProxyShape* shape1 = mDynamicAABBTree.getCollisionShape(pair->collisionShape1ID);
ProxyShape* shape2 = mDynamicAABBTree.getCollisionShape(pair->collisionShape2ID);
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);
@ -234,7 +241,7 @@ void BroadPhaseAlgorithm::computeOverlappingPairs() {
}
// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
void BroadPhaseAlgorithm::notifyOverlappingPair(int node1ID, int node2ID) {
void BroadPhaseAlgorithm::notifyOverlappingNodes(int node1ID, int node2ID) {
// If both the nodes are the same, we do not create store the overlapping pair
if (node1ID == node2ID) return;
@ -256,3 +263,30 @@ void BroadPhaseAlgorithm::notifyOverlappingPair(int node1ID, int 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);
}
// Called for a broad-phase shape that has to be tested for raycast
decimal BroadPhaseRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) {
decimal hitFraction = decimal(-1.0);
// Get the proxy shape from the node
ProxyShape* proxyShape = static_cast<ProxyShape*>(mDynamicAABBTree.getNodeDataPointer(nodeId));
// Check if the raycast filtering mask allows raycast against this shape
if ((mRaycastWithCategoryMaskBits & proxyShape->getCollisionCategoryBits()) != 0) {
// Ask the collision detection to perform a ray cast test against
// the proxy shape of this node because the ray is overlapping
// with the shape in the broad-phase
hitFraction = mRaycastTest.raycastAgainstShape(proxyShape, ray);
}
return hitFraction;
}

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

@ -31,12 +31,14 @@
#include "body/CollisionBody.h"
#include "collision/ProxyShape.h"
#include "DynamicAABBTree.h"
#include "engine/Profiler.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Declarations
class CollisionDetection;
class BroadPhaseAlgorithm;
// Structure BroadPhasePair
/**
@ -59,6 +61,59 @@ struct BroadPhasePair {
static bool smallerThan(const BroadPhasePair& pair1, const BroadPhasePair& pair2);
};
// class AABBOverlapCallback
class AABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
private:
BroadPhaseAlgorithm& mBroadPhaseAlgorithm;
int mReferenceNodeId;
public:
// Constructor
AABBOverlapCallback(BroadPhaseAlgorithm& broadPhaseAlgo, int referenceNodeId)
: mBroadPhaseAlgorithm(broadPhaseAlgo), mReferenceNodeId(referenceNodeId) {
}
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int nodeId);
};
// Class BroadPhaseRaycastCallback
/**
* Callback called when the AABB of a leaf node is hit by a ray the
* broad-phase Dynamic AABB Tree.
*/
class BroadPhaseRaycastCallback : public DynamicAABBTreeRaycastCallback {
private :
const DynamicAABBTree& mDynamicAABBTree;
unsigned short mRaycastWithCategoryMaskBits;
RaycastTest& mRaycastTest;
public:
// Constructor
BroadPhaseRaycastCallback(const DynamicAABBTree& dynamicAABBTree, unsigned short raycastWithCategoryMaskBits,
RaycastTest& raycastTest)
: mDynamicAABBTree(dynamicAABBTree), mRaycastWithCategoryMaskBits(raycastWithCategoryMaskBits),
mRaycastTest(raycastTest) {
}
// Called for a broad-phase shape that has to be tested for raycast
virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray);
};
// Class BroadPhaseAlgorithm
/**
* This class represents the broad-phase collision detection. The
@ -131,7 +186,7 @@ class BroadPhaseAlgorithm {
/// Notify the broad-phase that a collision shape has moved and need to be updated
void updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb,
const Vector3& displacement);
const Vector3& displacement, bool forceReinsert = false);
/// Add a collision shape in the array of shapes that have moved in the last simulation step
/// and that need to be tested again for broad-phase overlapping.
@ -142,7 +197,7 @@ class BroadPhaseAlgorithm {
void removeMovedCollisionShape(int broadPhaseID);
/// Notify the broad-phase about a potential overlapping pair in the dynamic AABB tree
void notifyOverlappingPair(int node1ID, int node2ID);
void notifyOverlappingNodes(int broadPhaseId1, int broadPhaseId2);
/// Compute all the overlapping pairs of collision shapes
void computeOverlappingPairs();
@ -179,7 +234,12 @@ inline bool BroadPhaseAlgorithm::testOverlappingShapes(const ProxyShape* shape1,
// Ray casting method
inline void BroadPhaseAlgorithm::raycast(const Ray& ray, RaycastTest& raycastTest,
unsigned short raycastWithCategoryMaskBits) const {
mDynamicAABBTree.raycast(ray, raycastTest, raycastWithCategoryMaskBits);
PROFILE("BroadPhaseAlgorithm::raycast()");
BroadPhaseRaycastCallback broadPhaseRaycastCallback(mDynamicAABBTree, raycastWithCategoryMaskBits, raycastTest);
mDynamicAABBTree.raycast(ray, broadPhaseRaycastCallback);
}
}

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

@ -35,7 +35,20 @@ using namespace reactphysics3d;
const int TreeNode::NULL_TREE_NODE = -1;
// Constructor
DynamicAABBTree::DynamicAABBTree(BroadPhaseAlgorithm& broadPhase) : mBroadPhase(broadPhase){
DynamicAABBTree::DynamicAABBTree(decimal extraAABBGap) : mExtraAABBGap(extraAABBGap) {
init();
}
// Destructor
DynamicAABBTree::~DynamicAABBTree() {
// Free the allocated memory for the nodes
free(mNodes);
}
// Initialize the tree
void DynamicAABBTree::init() {
mRootNodeID = TreeNode::NULL_TREE_NODE;
mNbNodes = 0;
@ -56,11 +69,14 @@ DynamicAABBTree::DynamicAABBTree(BroadPhaseAlgorithm& broadPhase) : mBroadPhase(
mFreeNodeID = 0;
}
// Destructor
DynamicAABBTree::~DynamicAABBTree() {
// Clear all the nodes and reset the tree
void DynamicAABBTree::reset() {
// Free the allocated memory for the nodes
free(mNodes);
// Initialize the tree
init();
}
// Allocate and return a new node in the tree
@ -93,9 +109,6 @@ int DynamicAABBTree::allocateNode() {
int freeNodeID = mFreeNodeID;
mFreeNodeID = mNodes[freeNodeID].nextNodeID;
mNodes[freeNodeID].parentID = TreeNode::NULL_TREE_NODE;
mNodes[freeNodeID].leftChildID = TreeNode::NULL_TREE_NODE;
mNodes[freeNodeID].rightChildID = TreeNode::NULL_TREE_NODE;
mNodes[freeNodeID].proxyShape = NULL;
mNodes[freeNodeID].height = 0;
mNbNodes++;
@ -114,21 +127,17 @@ void DynamicAABBTree::releaseNode(int nodeID) {
mNbNodes--;
}
// Add an object into the tree. This method creates a new leaf node in the tree and
// returns the ID of the corresponding node.
void DynamicAABBTree::addObject(ProxyShape* proxyShape, const AABB& aabb) {
// Internally add an object into the tree
int DynamicAABBTree::addObjectInternal(const AABB& aabb) {
// Get the next available node (or allocate new ones if necessary)
int nodeID = allocateNode();
// Create the fat aabb to use in the tree
const Vector3 gap(DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP);
const Vector3 gap(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap);
mNodes[nodeID].aabb.setMin(aabb.getMin() - gap);
mNodes[nodeID].aabb.setMax(aabb.getMax() + gap);
// Set the collision shape
mNodes[nodeID].proxyShape = proxyShape;
// Set the height of the node in the tree
mNodes[nodeID].height = 0;
@ -136,9 +145,10 @@ void DynamicAABBTree::addObject(ProxyShape* proxyShape, const AABB& aabb) {
insertLeafNode(nodeID);
assert(mNodes[nodeID].isLeaf());
// Set the broad-phase ID of the proxy shape
proxyShape->mBroadPhaseID = nodeID;
assert(nodeID >= 0);
// Return the Id of the node
return nodeID;
}
// Remove an object from the tree
@ -157,8 +167,9 @@ void DynamicAABBTree::removeObject(int nodeID) {
/// nothing is done. Otherwise, the corresponding node is removed and reinserted into the tree.
/// The method returns true if the object has been reinserted into the tree. The "displacement"
/// argument is the linear velocity of the AABB multiplied by the elapsed time between two
/// frames.
bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement) {
/// frames. If the "forceReinsert" parameter is true, we force a removal and reinsertion of the node
/// (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()");
@ -167,7 +178,7 @@ bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector
assert(mNodes[nodeID].height >= 0);
// If the new AABB is still inside the fat AABB of the node
if (mNodes[nodeID].aabb.contains(newAABB)) {
if (!forceReinsert && mNodes[nodeID].aabb.contains(newAABB)) {
return false;
}
@ -176,7 +187,7 @@ bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector
// Compute the fat AABB by inflating the AABB with a constant gap
mNodes[nodeID].aabb = newAABB;
const Vector3 gap(DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP);
const Vector3 gap(mExtraAABBGap, mExtraAABBGap, mExtraAABBGap);
mNodes[nodeID].aabb.mMinCoordinates -= gap;
mNodes[nodeID].aabb.mMaxCoordinates += gap;
@ -227,8 +238,8 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
int currentNodeID = mRootNodeID;
while (!mNodes[currentNodeID].isLeaf()) {
int leftChild = mNodes[currentNodeID].leftChildID;
int rightChild = mNodes[currentNodeID].rightChildID;
int leftChild = mNodes[currentNodeID].children[0];
int rightChild = mNodes[currentNodeID].children[1];
// Compute the merged AABB
decimal volumeAABB = mNodes[currentNodeID].aabb.getVolume();
@ -285,27 +296,27 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
int oldParentNode = mNodes[siblingNode].parentID;
int newParentNode = allocateNode();
mNodes[newParentNode].parentID = oldParentNode;
mNodes[newParentNode].proxyShape = NULL;
mNodes[newParentNode].aabb.mergeTwoAABBs(mNodes[siblingNode].aabb, newNodeAABB);
mNodes[newParentNode].height = mNodes[siblingNode].height + 1;
assert(mNodes[newParentNode].height > 0);
// If the sibling node was not the root node
if (oldParentNode != TreeNode::NULL_TREE_NODE) {
if (mNodes[oldParentNode].leftChildID == siblingNode) {
mNodes[oldParentNode].leftChildID = newParentNode;
assert(!mNodes[oldParentNode].isLeaf());
if (mNodes[oldParentNode].children[0] == siblingNode) {
mNodes[oldParentNode].children[0] = newParentNode;
}
else {
mNodes[oldParentNode].rightChildID = newParentNode;
mNodes[oldParentNode].children[1] = newParentNode;
}
mNodes[newParentNode].leftChildID = siblingNode;
mNodes[newParentNode].rightChildID = nodeID;
mNodes[newParentNode].children[0] = siblingNode;
mNodes[newParentNode].children[1] = nodeID;
mNodes[siblingNode].parentID = newParentNode;
mNodes[nodeID].parentID = newParentNode;
}
else { // If the sibling node was the root node
mNodes[newParentNode].leftChildID = siblingNode;
mNodes[newParentNode].rightChildID = nodeID;
mNodes[newParentNode].children[0] = siblingNode;
mNodes[newParentNode].children[1] = nodeID;
mNodes[siblingNode].parentID = newParentNode;
mNodes[nodeID].parentID = newParentNode;
mRootNodeID = newParentNode;
@ -313,14 +324,16 @@ void DynamicAABBTree::insertLeafNode(int nodeID) {
// Move up in the tree to change the AABBs that have changed
currentNodeID = mNodes[nodeID].parentID;
assert(!mNodes[currentNodeID].isLeaf());
while (currentNodeID != TreeNode::NULL_TREE_NODE) {
// Balance the sub-tree of the current node if it is not balanced
currentNodeID = balanceSubTreeAtNode(currentNodeID);
assert(mNodes[nodeID].isLeaf());
int leftChild = mNodes[currentNodeID].leftChildID;
int rightChild = mNodes[currentNodeID].rightChildID;
assert(!mNodes[currentNodeID].isLeaf());
int leftChild = mNodes[currentNodeID].children[0];
int rightChild = mNodes[currentNodeID].children[1];
assert(leftChild != TreeNode::NULL_TREE_NODE);
assert(rightChild != TreeNode::NULL_TREE_NODE);
@ -353,23 +366,23 @@ void DynamicAABBTree::removeLeafNode(int nodeID) {
int parentNodeID = mNodes[nodeID].parentID;
int grandParentNodeID = mNodes[parentNodeID].parentID;
int siblingNodeID;
if (mNodes[parentNodeID].leftChildID == nodeID) {
siblingNodeID = mNodes[parentNodeID].rightChildID;
if (mNodes[parentNodeID].children[0] == nodeID) {
siblingNodeID = mNodes[parentNodeID].children[1];
}
else {
siblingNodeID = mNodes[parentNodeID].leftChildID;
siblingNodeID = mNodes[parentNodeID].children[0];
}
// If the parent of the node to remove is not the root node
if (grandParentNodeID != TreeNode::NULL_TREE_NODE) {
// Destroy the parent node
if (mNodes[grandParentNodeID].leftChildID == parentNodeID) {
mNodes[grandParentNodeID].leftChildID = siblingNodeID;
if (mNodes[grandParentNodeID].children[0] == parentNodeID) {
mNodes[grandParentNodeID].children[0] = siblingNodeID;
}
else {
assert(mNodes[grandParentNodeID].rightChildID == parentNodeID);
mNodes[grandParentNodeID].rightChildID = siblingNodeID;
assert(mNodes[grandParentNodeID].children[1] == parentNodeID);
mNodes[grandParentNodeID].children[1] = siblingNodeID;
}
mNodes[siblingNodeID].parentID = grandParentNodeID;
releaseNode(parentNodeID);
@ -382,9 +395,11 @@ void DynamicAABBTree::removeLeafNode(int nodeID) {
// Balance the current sub-tree if necessary
currentNodeID = balanceSubTreeAtNode(currentNodeID);
assert(!mNodes[currentNodeID].isLeaf());
// Get the two children of the current node
int leftChildID = mNodes[currentNodeID].leftChildID;
int rightChildID = mNodes[currentNodeID].rightChildID;
int leftChildID = mNodes[currentNodeID].children[0];
int rightChildID = mNodes[currentNodeID].children[1];
// Recompute the AABB and the height of the current node
mNodes[currentNodeID].aabb.mergeTwoAABBs(mNodes[leftChildID].aabb,
@ -422,8 +437,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
}
// Get the two children nodes
int nodeBID = nodeA->leftChildID;
int nodeCID = nodeA->rightChildID;
int nodeBID = nodeA->children[0];
int nodeCID = nodeA->children[1];
assert(nodeBID >= 0 && nodeBID < mNbAllocatedNodes);
assert(nodeCID >= 0 && nodeCID < mNbAllocatedNodes);
TreeNode* nodeB = mNodes + nodeBID;
@ -435,35 +450,41 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
// If the right node C is 2 higher than left node B
if (balanceFactor > 1) {
int nodeFID = nodeC->leftChildID;
int nodeGID = nodeC->rightChildID;
assert(!nodeC->isLeaf());
int nodeFID = nodeC->children[0];
int nodeGID = nodeC->children[1];
assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes);
TreeNode* nodeF = mNodes + nodeFID;
TreeNode* nodeG = mNodes + nodeGID;
nodeC->leftChildID = nodeID;
nodeC->children[0] = nodeID;
nodeC->parentID = nodeA->parentID;
nodeA->parentID = nodeCID;
if (nodeC->parentID != TreeNode::NULL_TREE_NODE) {
if (mNodes[nodeC->parentID].leftChildID == nodeID) {
mNodes[nodeC->parentID].leftChildID = nodeCID;
if (mNodes[nodeC->parentID].children[0] == nodeID) {
mNodes[nodeC->parentID].children[0] = nodeCID;
}
else {
assert(mNodes[nodeC->parentID].rightChildID == nodeID);
mNodes[nodeC->parentID].rightChildID = nodeCID;
assert(mNodes[nodeC->parentID].children[1] == nodeID);
mNodes[nodeC->parentID].children[1] = nodeCID;
}
}
else {
mRootNodeID = nodeCID;
}
assert(!nodeC->isLeaf());
assert(!nodeA->isLeaf());
// If the right node C was higher than left node B because of the F node
if (nodeF->height > nodeG->height) {
nodeC->rightChildID = nodeFID;
nodeA->rightChildID = nodeGID;
nodeC->children[1] = nodeFID;
nodeA->children[1] = nodeGID;
nodeG->parentID = nodeID;
// Recompute the AABB of node A and C
@ -477,8 +498,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
assert(nodeC->height > 0);
}
else { // If the right node C was higher than left node B because of node G
nodeC->rightChildID = nodeGID;
nodeA->rightChildID = nodeFID;
nodeC->children[1] = nodeGID;
nodeA->children[1] = nodeFID;
nodeF->parentID = nodeID;
// Recompute the AABB of node A and C
@ -499,35 +520,41 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
// If the left node B is 2 higher than right node C
if (balanceFactor < -1) {
int nodeFID = nodeB->leftChildID;
int nodeGID = nodeB->rightChildID;
assert(!nodeB->isLeaf());
int nodeFID = nodeB->children[0];
int nodeGID = nodeB->children[1];
assert(nodeFID >= 0 && nodeFID < mNbAllocatedNodes);
assert(nodeGID >= 0 && nodeGID < mNbAllocatedNodes);
TreeNode* nodeF = mNodes + nodeFID;
TreeNode* nodeG = mNodes + nodeGID;
nodeB->leftChildID = nodeID;
nodeB->children[0] = nodeID;
nodeB->parentID = nodeA->parentID;
nodeA->parentID = nodeBID;
if (nodeB->parentID != TreeNode::NULL_TREE_NODE) {
if (mNodes[nodeB->parentID].leftChildID == nodeID) {
mNodes[nodeB->parentID].leftChildID = nodeBID;
if (mNodes[nodeB->parentID].children[0] == nodeID) {
mNodes[nodeB->parentID].children[0] = nodeBID;
}
else {
assert(mNodes[nodeB->parentID].rightChildID == nodeID);
mNodes[nodeB->parentID].rightChildID = nodeBID;
assert(mNodes[nodeB->parentID].children[1] == nodeID);
mNodes[nodeB->parentID].children[1] = nodeBID;
}
}
else {
mRootNodeID = nodeBID;
}
assert(!nodeB->isLeaf());
assert(!nodeA->isLeaf());
// If the left node B was higher than right node C because of the F node
if (nodeF->height > nodeG->height) {
nodeB->rightChildID = nodeFID;
nodeA->leftChildID = nodeGID;
nodeB->children[1] = nodeFID;
nodeA->children[0] = nodeGID;
nodeG->parentID = nodeID;
// Recompute the AABB of node A and B
@ -541,8 +568,8 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
assert(nodeB->height > 0);
}
else { // If the left node B was higher than right node C because of node G
nodeB->rightChildID = nodeGID;
nodeA->leftChildID = nodeFID;
nodeB->children[1] = nodeGID;
nodeA->children[0] = nodeFID;
nodeF->parentID = nodeID;
// Recompute the AABB of node A and B
@ -564,11 +591,9 @@ int DynamicAABBTree::balanceSubTreeAtNode(int nodeID) {
return nodeID;
}
// Report all shapes overlapping with the AABB given in parameter.
/// For each overlapping shape with the AABB given in parameter, the
/// BroadPhase::notifyOverlappingPair() method is called to store a
/// potential overlapping pair.
void DynamicAABBTree::reportAllShapesOverlappingWith(int nodeID, const AABB& aabb) {
/// Report all shapes overlapping with the AABB given in parameter.
void DynamicAABBTree::reportAllShapesOverlappingWithAABB(const AABB& aabb,
DynamicAABBTreeOverlapCallback& callback) const {
// Create a stack with the nodes to visit
Stack<int, 64> stack;
@ -593,30 +618,25 @@ void DynamicAABBTree::reportAllShapesOverlappingWith(int nodeID, const AABB& aab
if (nodeToVisit->isLeaf()) {
// Notify the broad-phase about a new potential overlapping pair
mBroadPhase.notifyOverlappingPair(nodeID, nodeIDToVisit);
callback.notifyOverlappingNode(nodeIDToVisit);
}
else { // If the node is not a leaf
// We need to visit its children
stack.push(nodeToVisit->leftChildID);
stack.push(nodeToVisit->rightChildID);
stack.push(nodeToVisit->children[0]);
stack.push(nodeToVisit->children[1]);
}
}
}
}
// Ray casting method
void DynamicAABBTree::raycast(const Ray& ray, RaycastTest& raycastTest,
unsigned short raycastWithCategoryMaskBits) const {
void DynamicAABBTree::raycast(const Ray& ray, DynamicAABBTreeRaycastCallback &callback) const {
PROFILE("DynamicAABBTree::raycast()");
decimal maxFraction = ray.maxFraction;
// Create an AABB for the ray
Vector3 endPoint = ray.point1 +
maxFraction * (ray.point2 - ray.point1);
AABB rayAABB(Vector3::min(ray.point1, endPoint),
Vector3::max(ray.point1, endPoint));
Stack<int, 128> stack;
stack.push(mRootNodeID);
@ -633,52 +653,41 @@ void DynamicAABBTree::raycast(const Ray& ray, RaycastTest& raycastTest,
// Get the corresponding node
const TreeNode* node = mNodes + nodeID;
// Test if the node AABB overlaps with the ray AABB
if (!rayAABB.testCollision(node->aabb)) continue;
Ray rayTemp(ray.point1, ray.point2, maxFraction);
// Test if the ray intersects with the current node AABB
if (!node->aabb.testRayIntersect(rayTemp)) continue;
// If the node is a leaf of the tree
if (node->isLeaf()) {
// Check if the raycast filtering mask allows raycast against this shape
if ((raycastWithCategoryMaskBits & node->proxyShape->getCollisionCategoryBits()) != 0) {
// Call the callback that will raycast again the broad-phase shape
decimal hitFraction = callback.raycastBroadPhaseShape(nodeID, rayTemp);
Ray rayTemp(ray.point1, ray.point2, maxFraction);
// Ask the collision detection to perform a ray cast test against
// the proxy shape of this node because the ray is overlapping
// with the shape in the broad-phase
decimal hitFraction = raycastTest.raycastAgainstShape(node->proxyShape,
rayTemp);
// If the user returned a hitFraction of zero, it means that
// the raycasting should stop here
if (hitFraction == decimal(0.0)) {
return;
}
// If the user returned a positive fraction
if (hitFraction > decimal(0.0)) {
// We update the maxFraction value and the ray
// AABB using the new maximum fraction
if (hitFraction < maxFraction) {
maxFraction = hitFraction;
}
endPoint = ray.point1 + maxFraction * (ray.point2 - ray.point1);
rayAABB.mMinCoordinates = Vector3::min(ray.point1, endPoint);
rayAABB.mMaxCoordinates = Vector3::max(ray.point1, endPoint);
}
// If the user returned a negative fraction, we continue
// the raycasting as if the proxy shape did not exist
// If the user returned a hitFraction of zero, it means that
// the raycasting should stop here
if (hitFraction == decimal(0.0)) {
return;
}
// If the user returned a positive fraction
if (hitFraction > decimal(0.0)) {
// We update the maxFraction value and the ray
// AABB using the new maximum fraction
if (hitFraction < maxFraction) {
maxFraction = hitFraction;
}
}
// If the user returned a negative fraction, we continue
// the raycasting as if the proxy shape did not exist
}
else { // If the node has children
// Push its children in the stack of nodes to explore
stack.push(node->leftChildID);
stack.push(node->rightChildID);
stack.push(node->children[0]);
stack.push(node->children[1]);
}
}
}
@ -716,8 +725,9 @@ void DynamicAABBTree::checkNode(int nodeID) const {
// Get the children nodes
TreeNode* pNode = mNodes + nodeID;
int leftChild = pNode->leftChildID;
int rightChild = pNode->rightChildID;
assert(!pNode->isLeaf());
int leftChild = pNode->children[0];
int rightChild = pNode->children[1];
assert(pNode->height >= 0);
assert(pNode->aabb.getVolume() > 0);
@ -772,8 +782,8 @@ int DynamicAABBTree::computeHeight(int nodeID) {
}
// Compute the height of the left and right sub-tree
int leftHeight = computeHeight(node->leftChildID);
int rightHeight = computeHeight(node->rightChildID);
int leftHeight = computeHeight(node->children[0]);
int rightHeight = computeHeight(node->children[1]);
// Return the height of the node
return 1 + std::max(leftHeight, rightHeight);

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

@ -36,13 +36,10 @@ namespace reactphysics3d {
// Declarations
class BroadPhaseAlgorithm;
class BroadPhaseRaycastTestCallback;
class DynamicAABBTreeOverlapCallback;
struct RaycastTest;
// Raycast callback method pointer type
typedef decimal (*RaycastTestCallback) (ProxyShape* shape,
RaycastCallback* userCallback,
const Ray& ray);
// Structure TreeNode
/**
* This structure represents a node of the dynamic AABB tree.
@ -56,30 +53,70 @@ struct TreeNode {
// -------------------- Attributes -------------------- //
/// Parent node ID
int parentID;
// A node is either in the tree (has a parent) or in the free nodes list
// (has a next node)
union {
/// Left and right child of the node
int leftChildID, rightChildID;
/// Parent node ID
int32 parentID;
/// Next allocated node ID
int nextNodeID;
/// Next allocated node ID
int32 nextNodeID;
};
// A node is either a leaf (has data) or is an internal node (has children)
union {
/// Left and right child of the node (children[0] = left child)
int32 children[2];
/// Two pieces of data stored at that node (in case the node is a leaf)
union {
int32 dataInt[2];
void* dataPointer;
};
};
/// Height of the node in the tree
int height;
int16 height;
/// Fat axis aligned bounding box (AABB) corresponding to the node
AABB aabb;
/// Pointer to the corresponding collision shape (in case this node is a leaf)
ProxyShape* proxyShape;
// -------------------- Methods -------------------- //
/// Return true if the node is a leaf of the tree
bool isLeaf() const;
};
// Class DynamicAABBTreeOverlapCallback
/**
* Overlapping callback method that has to be used as parameter of the
* reportAllShapesOverlappingWithNode() method.
*/
class DynamicAABBTreeOverlapCallback {
public :
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int nodeId)=0;
};
// Class DynamicAABBTreeRaycastCallback
/**
* Raycast callback in the Dynamic AABB Tree called when the AABB of a leaf
* node is hit by the ray.
*/
class DynamicAABBTreeRaycastCallback {
public:
// Called when the AABB of a leaf node is hit by a ray
virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray)=0;
};
// Class DynamicAABBTree
/**
* This class implements a dynamic AABB tree that is used for broad-phase
@ -94,9 +131,6 @@ class DynamicAABBTree {
// -------------------- Attributes -------------------- //
/// Reference to the broad-phase
BroadPhaseAlgorithm& mBroadPhase;
/// Pointer to the memory location of the nodes of the tree
TreeNode* mNodes;
@ -112,6 +146,10 @@ class DynamicAABBTree {
/// Number of nodes in the tree
int mNbNodes;
/// Extra AABB Gap used to allow the collision shape to move a little bit
/// without triggering a large modification of the tree which can be costly
decimal mExtraAABBGap;
// -------------------- Methods -------------------- //
/// Allocate and return a node to use in the tree
@ -132,6 +170,12 @@ class DynamicAABBTree {
/// Compute the height of a given node in the tree
int computeHeight(int nodeID);
/// Internally add an object into the tree
int addObjectInternal(const AABB& aabb);
/// Initialize the tree
void init();
#ifndef NDEBUG
/// Check if the tree structure is valid (for debugging purpose)
@ -147,40 +191,52 @@ class DynamicAABBTree {
// -------------------- Methods -------------------- //
/// Constructor
DynamicAABBTree(BroadPhaseAlgorithm& broadPhase);
DynamicAABBTree(decimal extraAABBGap = decimal(0.0));
/// Destructor
~DynamicAABBTree();
/// Add an object into the tree
void addObject(ProxyShape* proxyShape, const AABB& aabb);
/// Add an object into the tree (where node data are two integers)
int addObject(const AABB& aabb, int32 data1, int32 data2);
/// Add an object into the tree (where node data is a pointer)
int addObject(const AABB& aabb, void* data);
/// Remove an object from the tree
void removeObject(int nodeID);
/// Update the dynamic tree after an object has moved.
bool updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement);
bool updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement, bool forceReinsert = false);
/// Return the fat AABB corresponding to a given node ID
const AABB& getFatAABB(int nodeID) const;
/// Return the collision shape of a given leaf node of the tree
ProxyShape* getCollisionShape(int nodeID) const;
/// Return the pointer to the data array of a given leaf node of the tree
int32* getNodeDataInt(int nodeID) const;
/// Return the data pointer of a given leaf node of the tree
void* getNodeDataPointer(int nodeID) const;
/// Report all shapes overlapping with the AABB given in parameter.
void reportAllShapesOverlappingWith(int nodeID, const AABB& aabb);
void reportAllShapesOverlappingWithAABB(const AABB& aabb,
DynamicAABBTreeOverlapCallback& callback) const;
/// Ray casting method
void raycast(const Ray& ray, RaycastTest& raycastTest,
unsigned short raycastWithCategoryMaskBits) const;
void raycast(const Ray& ray, DynamicAABBTreeRaycastCallback& callback) const;
/// Compute the height of the tree
int computeHeight();
/// Return the root AABB of the tree
AABB getRootAABB() const;
/// Clear all the nodes and reset the tree
void reset();
};
// Return true if the node is a leaf of the tree
inline bool TreeNode::isLeaf() const {
return leftChildID == NULL_TREE_NODE;
return (height == 0);
}
// Return the fat AABB corresponding to a given node ID
@ -189,11 +245,46 @@ inline const AABB& DynamicAABBTree::getFatAABB(int nodeID) const {
return mNodes[nodeID].aabb;
}
// Return the collision shape of a given leaf node of the tree
inline ProxyShape* DynamicAABBTree::getCollisionShape(int nodeID) const {
// Return the pointer to the data array of a given leaf node of the tree
inline int32* DynamicAABBTree::getNodeDataInt(int nodeID) const {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf());
return mNodes[nodeID].proxyShape;
return mNodes[nodeID].dataInt;
}
// Return the pointer to the data pointer of a given leaf node of the tree
inline void* DynamicAABBTree::getNodeDataPointer(int nodeID) const {
assert(nodeID >= 0 && nodeID < mNbAllocatedNodes);
assert(mNodes[nodeID].isLeaf());
return mNodes[nodeID].dataPointer;
}
// Return the root AABB of the tree
inline AABB DynamicAABBTree::getRootAABB() const {
return getFatAABB(mRootNodeID);
}
// Add an object into the tree. This method creates a new leaf node in the tree and
// returns the ID of the corresponding node.
inline int DynamicAABBTree::addObject(const AABB& aabb, int32 data1, int32 data2) {
int nodeId = addObjectInternal(aabb);
mNodes[nodeId].dataInt[0] = data1;
mNodes[nodeId].dataInt[1] = data2;
return nodeId;
}
// Add an object into the tree. This method creates a new leaf node in the tree and
// returns the ID of the corresponding node.
inline int DynamicAABBTree::addObject(const AABB& aabb, void* data) {
int nodeId = addObjectInternal(aabb);
mNodes[nodeId].dataPointer = data;
return nodeId;
}
}

67
src/collision/narrowphase/CollisionDispatch.h Normal file
View File

@ -0,0 +1,67 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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_DISPATCH_H
#define REACTPHYSICS3D_COLLISION_DISPATCH_H
// Libraries
#include "NarrowPhaseAlgorithm.h"
namespace reactphysics3d {
// Class CollisionDispatch
/**
* This is the abstract base class for dispatching the narrow-phase
* collision detection algorithm. Collision dispatching decides which collision
* algorithm to use given two types of proxy collision shapes.
*/
class CollisionDispatch {
protected:
public:
/// Constructor
CollisionDispatch() {}
/// Destructor
virtual ~CollisionDispatch() {}
/// 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;
};
}
#endif

291
src/collision/narrowphase/ConcaveVsConvexAlgorithm.cpp Normal file
View File

@ -0,0 +1,291 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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;
// Constructor
ConcaveVsConvexAlgorithm::ConcaveVsConvexAlgorithm() {
}
// Destructor
ConcaveVsConvexAlgorithm::~ConcaveVsConvexAlgorithm() {
}
// 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 == NULL) 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() == TRIANGLE) {
assert(contactInfo.collisionShape2->getType() != 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() == 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);
}

234
src/collision/narrowphase/ConcaveVsConvexAlgorithm.h Normal file
View File

@ -0,0 +1,234 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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:
/// 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);
};
// 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);
};
// 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 -------------------- //
/// Private copy-constructor
ConcaveVsConvexAlgorithm(const ConcaveVsConvexAlgorithm& algorithm);
/// Private assignment operator
ConcaveVsConvexAlgorithm& operator=(const ConcaveVsConvexAlgorithm& algorithm);
/// 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();
/// Destructor
virtual ~ConcaveVsConvexAlgorithm();
/// Compute a contact info if the two bounding volume collide
virtual void testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback);
};
// 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

75
src/collision/narrowphase/DefaultCollisionDispatch.cpp Normal file
View File

@ -0,0 +1,75 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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 "DefaultCollisionDispatch.h"
#include "collision/shapes/CollisionShape.h"
using namespace reactphysics3d;
// Constructor
DefaultCollisionDispatch::DefaultCollisionDispatch() {
}
// Destructor
DefaultCollisionDispatch::~DefaultCollisionDispatch() {
}
/// 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) {
CollisionShapeType shape1Type = static_cast<CollisionShapeType>(type1);
CollisionShapeType shape2Type = static_cast<CollisionShapeType>(type2);
// Sphere vs Sphere algorithm
if (shape1Type == SPHERE && shape2Type == SPHERE) {
return &mSphereVsSphereAlgorithm;
}
// Concave vs Convex algorithm
else if ((!CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) ||
(!CollisionShape::isConvex(shape2Type) && CollisionShape::isConvex(shape1Type))) {
return &mConcaveVsConvexAlgorithm;
}
// Convex vs Convex algorithm (GJK algorithm)
else if (CollisionShape::isConvex(shape1Type) && CollisionShape::isConvex(shape2Type)) {
return &mGJKAlgorithm;
}
else {
return NULL;
}
}

78
src/collision/narrowphase/DefaultCollisionDispatch.h Normal file
View File

@ -0,0 +1,78 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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_DEFAULT_COLLISION_DISPATCH_H
#define REACTPHYSICS3D_DEFAULT_COLLISION_DISPATCH_H
// Libraries
#include "CollisionDispatch.h"
#include "ConcaveVsConvexAlgorithm.h"
#include "SphereVsSphereAlgorithm.h"
#include "GJK/GJKAlgorithm.h"
namespace reactphysics3d {
// Class DefaultCollisionDispatch
/**
* This is the default collision dispatch configuration use in ReactPhysics3D.
* Collision dispatching decides which collision
* algorithm to use given two types of proxy collision shapes.
*/
class DefaultCollisionDispatch : public CollisionDispatch {
protected:
/// Sphere vs Sphere collision algorithm
SphereVsSphereAlgorithm mSphereVsSphereAlgorithm;
/// Concave vs Convex collision algorithm
ConcaveVsConvexAlgorithm mConcaveVsConvexAlgorithm;
/// GJK Algorithm
GJKAlgorithm mGJKAlgorithm;
public:
/// Constructor
DefaultCollisionDispatch();
/// Destructor
virtual ~DefaultCollisionDispatch();
/// 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 type1, int type2);
};
}
#endif

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

@ -25,6 +25,7 @@
// Libraries
#include "EPAAlgorithm.h"
#include "engine/Profiler.h"
#include "collision/narrowphase//GJK/GJKAlgorithm.h"
#include "TrianglesStore.h"
@ -32,8 +33,7 @@
using namespace reactphysics3d;
// Constructor
EPAAlgorithm::EPAAlgorithm(MemoryAllocator& memoryAllocator)
: mMemoryAllocator(memoryAllocator) {
EPAAlgorithm::EPAAlgorithm() {
}
@ -82,12 +82,24 @@ int EPAAlgorithm::isOriginInTetrahedron(const Vector3& p1, const Vector3& p2,
/// 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
bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex,
ProxyShape* collisionShape1,
void EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simplex,
CollisionShapeInfo shape1Info,
const Transform& transform1,
ProxyShape* collisionShape2,
CollisionShapeInfo shape2Info,
const Transform& transform2,
Vector3& v, ContactPointInfo*& contactInfo) {
Vector3& v,
NarrowPhaseCallback* narrowPhaseCallback) {
PROFILE("EPAAlgorithm::computePenetrationDepthAndContactPoints()");
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
@ -125,7 +137,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// Only one point in the simplex (which should be the origin).
// We have a touching contact with zero penetration depth.
// We drop that kind of contact. Therefore, we return false
return false;
return;
case 2: {
// The simplex returned by GJK is a line segment d containing the origin.
@ -155,21 +167,21 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
Vector3 v3 = rotationQuat * v2;
// Compute the support point in the direction of v1
suppPointsA[2] = collisionShape1->getLocalSupportPointWithMargin(v1);
suppPointsA[2] = shape1->getLocalSupportPointWithMargin(v1, shape1CachedCollisionData);
suppPointsB[2] = body2Tobody1 *
collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1));
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1), shape2CachedCollisionData);
points[2] = suppPointsA[2] - suppPointsB[2];
// Compute the support point in the direction of v2
suppPointsA[3] = collisionShape1->getLocalSupportPointWithMargin(v2);
suppPointsA[3] = shape1->getLocalSupportPointWithMargin(v2, shape1CachedCollisionData);
suppPointsB[3] = body2Tobody1 *
collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2));
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2), shape2CachedCollisionData);
points[3] = suppPointsA[3] - suppPointsB[3];
// Compute the support point in the direction of v3
suppPointsA[4] = collisionShape1->getLocalSupportPointWithMargin(v3);
suppPointsA[4] = shape1->getLocalSupportPointWithMargin(v3, shape1CachedCollisionData);
suppPointsB[4] = body2Tobody1 *
collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v3));
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
@ -192,7 +204,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
}
else {
// The origin is not in the initial polytope
return false;
return;
}
// The polytope contains now 4 vertices
@ -222,7 +234,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL)
&& face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0
&& face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
return false;
return;
}
// Associate the edges of neighbouring triangle faces
@ -268,13 +280,13 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
Vector3 n = v1.cross(v2);
// Compute the two new vertices to obtain a hexahedron
suppPointsA[3] = collisionShape1->getLocalSupportPointWithMargin(n);
suppPointsA[3] = shape1->getLocalSupportPointWithMargin(n, shape1CachedCollisionData);
suppPointsB[3] = body2Tobody1 *
collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * (-n));
shape2->getLocalSupportPointWithMargin(rotateToBody2 * (-n), shape2CachedCollisionData);
points[3] = suppPointsA[3] - suppPointsB[3];
suppPointsA[4] = collisionShape1->getLocalSupportPointWithMargin(-n);
suppPointsA[4] = shape1->getLocalSupportPointWithMargin(-n, shape1CachedCollisionData);
suppPointsB[4] = body2Tobody1 *
collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * n);
shape2->getLocalSupportPointWithMargin(rotateToBody2 * n, shape2CachedCollisionData);
points[4] = suppPointsA[4] - suppPointsB[4];
TriangleEPA* face0 = NULL;
@ -307,14 +319,14 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
face3 = triangleStore.newTriangle(points, 1, 4, 2);
}
else {
return false;
return;
}
// If the constructed tetrahedron is not correct
if (!((face0 != NULL) && (face1 != NULL) && (face2 != NULL) && (face3 != NULL)
&& face0->getDistSquare() > 0.0 && face1->getDistSquare() > 0.0
&& face2->getDistSquare() > 0.0 && face3->getDistSquare() > 0.0)) {
return false;
return;
}
// Associate the edges of neighbouring triangle faces
@ -341,7 +353,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// can run the EPA algorithm.
if (nbTriangles == 0) {
return false;
return;
}
TriangleEPA* triangle = 0;
@ -364,11 +376,11 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// Compute the support point of the Minkowski
// difference (A-B) in the closest point direction
suppPointsA[nbVertices] = collisionShape1->getLocalSupportPointWithMargin(
triangle->getClosestPoint());
suppPointsA[nbVertices] = shape1->getLocalSupportPointWithMargin(
triangle->getClosestPoint(), shape1CachedCollisionData);
suppPointsB[nbVertices] = body2Tobody1 *
collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 *
(-triangle->getClosestPoint()));
shape2->getLocalSupportPointWithMargin(rotateToBody2 *
(-triangle->getClosestPoint()), shape2CachedCollisionData);
points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
int indexNewVertex = nbVertices;
@ -416,11 +428,12 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
Vector3 normal = v.getUnit();
decimal penetrationDepth = v.length();
assert(penetrationDepth > 0.0);
if (normal.lengthSquare() < MACHINE_EPSILON) return;
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(collisionShape1, collisionShape2, normal,
penetrationDepth, pALocal, pBLocal);
return true;
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pALocal, pBLocal);
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
}

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

@ -29,7 +29,9 @@
// Libraries
#include "collision/narrowphase/GJK/Simplex.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"
@ -86,7 +88,7 @@ class EPAAlgorithm {
// -------------------- Attributes -------------------- //
/// Reference to the memory allocator
MemoryAllocator& mMemoryAllocator;
MemoryAllocator* mMemoryAllocator;
/// Triangle comparison operator
TriangleComparison mTriangleComparison;
@ -112,18 +114,22 @@ class EPAAlgorithm {
// -------------------- Methods -------------------- //
/// Constructor
EPAAlgorithm(MemoryAllocator& memoryAllocator);
EPAAlgorithm();
/// Destructor
~EPAAlgorithm();
/// Initalize the algorithm
void init(MemoryAllocator* memoryAllocator);
/// Compute the penetration depth with EPA algorithm.
bool computePenetrationDepthAndContactPoints(const Simplex& simplex,
ProxyShape* collisionShape1,
void computePenetrationDepthAndContactPoints(const Simplex& simplex,
CollisionShapeInfo shape1Info,
const Transform& transform1,
ProxyShape* collisionShape2,
CollisionShapeInfo shape2Info,
const Transform& transform2,
Vector3& v, ContactPointInfo*& contactInfo);
Vector3& v,
NarrowPhaseCallback* narrowPhaseCallback);
};
// Add a triangle face in the candidate triangle heap in the EPA algorithm
@ -144,6 +150,11 @@ inline void EPAAlgorithm::addFaceCandidate(TriangleEPA* triangle, TriangleEPA**
}
}
// Initalize the algorithm
inline void EPAAlgorithm::init(MemoryAllocator* memoryAllocator) {
mMemoryAllocator = memoryAllocator;
}
}
#endif

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

@ -28,6 +28,7 @@
#include "Simplex.h"
#include "constraint/ContactPoint.h"
#include "configuration.h"
#include "engine/Profiler.h"
#include <algorithm>
#include <cmath>
#include <cfloat>
@ -37,9 +38,8 @@
using namespace reactphysics3d;
// Constructor
GJKAlgorithm::GJKAlgorithm(MemoryAllocator& memoryAllocator)
:NarrowPhaseAlgorithm(memoryAllocator), mAlgoEPA(memoryAllocator) {
GJKAlgorithm::GJKAlgorithm() : NarrowPhaseAlgorithm() {
}
// Destructor
@ -47,18 +47,20 @@ GJKAlgorithm::~GJKAlgorithm() {
}
// Return true and compute a contact info if the two collision shapes collide.
// 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 objects don't intersect, this method returns false. If they intersect
/// running the GJK algorithm on original objects (without margin). If the shapes intersect
/// only in the margins, the method compute the penetration depth and contact points
/// (of enlarged objects). If the original objects (without margin) intersect, we
/// call the computePenetrationDepthForEnlargedObjects() method that run the GJK
/// 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.
bool GJKAlgorithm::testCollision(ProxyShape* collisionShape1, ProxyShape* collisionShape2,
ContactPointInfo*& contactInfo) {
void GJKAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) {
PROFILE("GJKAlgorithm::testCollision()");
Vector3 suppA; // Support point of object A
Vector3 suppB; // Support point of object B
@ -68,11 +70,18 @@ bool GJKAlgorithm::testCollision(ProxyShape* collisionShape1, ProxyShape* collis
decimal vDotw;
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);
void** shape1CachedCollisionData = shape1Info.cachedCollisionData;
void** shape2CachedCollisionData = shape2Info.cachedCollisionData;
// Get the local-space to world-space transforms
const Transform transform1 = collisionShape1->getBody()->getTransform() *
collisionShape1->getLocalToBodyTransform();
const Transform transform2 = collisionShape2->getBody()->getTransform() *
collisionShape2->getLocalToBodyTransform();
const Transform transform1 = shape1Info.shapeToWorldTransform;
const Transform transform2 = shape2Info.shapeToWorldTransform;
// 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)
@ -84,7 +93,7 @@ bool GJKAlgorithm::testCollision(ProxyShape* collisionShape1, ProxyShape* collis
transform1.getOrientation().getMatrix();
// Initialize the margin (sum of margins of both objects)
decimal margin = collisionShape1->getMargin() + collisionShape2->getMargin();
decimal margin = shape1->getMargin() + shape2->getMargin();
decimal marginSquare = margin * margin;
assert(margin > 0.0);
@ -100,9 +109,9 @@ bool GJKAlgorithm::testCollision(ProxyShape* collisionShape1, ProxyShape* collis
do {
// Compute the support points for original objects (without margins) A and B
suppA = collisionShape1->getLocalSupportPointWithoutMargin(-v);
suppA = shape1->getLocalSupportPointWithoutMargin(-v, shape1CachedCollisionData);
suppB = body2Tobody1 *
collisionShape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v);
shape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v, shape2CachedCollisionData);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
@ -115,8 +124,8 @@ bool GJKAlgorithm::testCollision(ProxyShape* collisionShape1, ProxyShape* collis
// Cache the current separating axis for frame coherence
mCurrentOverlappingPair->setCachedSeparatingAxis(v);
// No intersection, we return false
return false;
// No intersection, we return
return;
}
// If the objects intersect only in the margins
@ -129,23 +138,24 @@ bool GJKAlgorithm::testCollision(ProxyShape* collisionShape1, ProxyShape* collis
// object with the margins
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (collisionShape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return false;
if (penetrationDepth <= 0.0) return;
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(collisionShape1, collisionShape2, normal,
penetrationDepth, pA, pB);
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
// There is an intersection, therefore we return true
return true;
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return
return;
}
// Add the new support point to the simplex
@ -161,23 +171,24 @@ bool GJKAlgorithm::testCollision(ProxyShape* collisionShape1, ProxyShape* collis
// object with the margins
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (collisionShape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return false;
if (penetrationDepth <= 0.0) return;
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(collisionShape1, collisionShape2, normal,
penetrationDepth, pA, pB);
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
// There is an intersection, therefore we return true
return true;
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return
return;
}
// Compute the point of the simplex closest to the origin
@ -191,23 +202,24 @@ bool GJKAlgorithm::testCollision(ProxyShape* collisionShape1, ProxyShape* collis
// object with the margins
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (collisionShape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return false;
if (penetrationDepth <= 0.0) return;
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(collisionShape1, collisionShape2, normal,
penetrationDepth, pA, pB);
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
// There is an intersection, therefore we return true
return true;
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return
return;
}
// Store and update the squared distance of the closest point
@ -228,23 +240,24 @@ bool GJKAlgorithm::testCollision(ProxyShape* collisionShape1, ProxyShape* collis
// object with the margins
decimal dist = sqrt(distSquare);
assert(dist > 0.0);
pA = (pA - (collisionShape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (collisionShape2->getMargin() / dist) * v);
pA = (pA - (shape1->getMargin() / dist) * v);
pB = body2Tobody1.getInverse() * (pB + (shape2->getMargin() / dist) * v);
// Compute the contact info
Vector3 normal = transform1.getOrientation() * (-v.getUnit());
decimal penetrationDepth = margin - dist;
// Reject the contact if the penetration depth is negative (due too numerical errors)
if (penetrationDepth <= 0.0) return false;
if (penetrationDepth <= 0.0) return;
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(collisionShape1, collisionShape2, normal,
penetrationDepth, pA, pB);
ContactPointInfo contactInfo(shape1Info.proxyShape, shape2Info.proxyShape, shape1Info.collisionShape,
shape2Info.collisionShape, normal, penetrationDepth, pA, pB);
// There is an intersection, therefore we return true
return true;
narrowPhaseCallback->notifyContact(shape1Info.overlappingPair, contactInfo);
// There is an intersection, therefore we return
return;
}
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
simplex.getMaxLengthSquareOfAPoint());
@ -253,8 +266,8 @@ bool GJKAlgorithm::testCollision(ProxyShape* collisionShape1, ProxyShape* collis
// again but on the enlarged objects to compute a simplex polytope that contains
// the origin. Then, we give that simplex polytope to the EPA algorithm to compute
// the correct penetration depth and contact points between the enlarged objects.
return computePenetrationDepthForEnlargedObjects(collisionShape1, transform1, collisionShape2,
transform2, contactInfo, v);
return computePenetrationDepthForEnlargedObjects(shape1Info, transform1, shape2Info,
transform2, narrowPhaseCallback, v);
}
/// This method runs the GJK algorithm on the two enlarged objects (with margin)
@ -262,12 +275,14 @@ bool GJKAlgorithm::testCollision(ProxyShape* collisionShape1, ProxyShape* collis
/// 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(ProxyShape* collisionShape1,
void GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
const Transform& transform1,
ProxyShape* collisionShape2,
const CollisionShapeInfo& shape2Info,
const Transform& transform2,
ContactPointInfo*& contactInfo,
NarrowPhaseCallback* narrowPhaseCallback,
Vector3& v) {
PROFILE("GJKAlgorithm::computePenetrationDepthForEnlargedObjects()");
Simplex simplex;
Vector3 suppA;
Vector3 suppB;
@ -276,6 +291,15 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(ProxyShape* collisi
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);
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;
@ -286,8 +310,8 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(ProxyShape* collisi
do {
// Compute the support points for the enlarged object A and B
suppA = collisionShape1->getLocalSupportPointWithMargin(-v);
suppB = body2ToBody1 * collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * v);
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;
@ -297,19 +321,19 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(ProxyShape* collisi
// If the enlarge objects do not intersect
if (vDotw > 0.0) {
// No intersection, we return false
return false;
// No intersection, we return
return;
}
// Add the new support point to the simplex
simplex.addPoint(w, suppA, suppB);
if (simplex.isAffinelyDependent()) {
return false;
return;
}
if (!simplex.computeClosestPoint(v)) {
return false;
return;
}
// Store and update the square distance
@ -317,7 +341,7 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(ProxyShape* collisi
distSquare = v.lengthSquare();
if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
return false;
return;
}
} while(!simplex.isFull() && distSquare > MACHINE_EPSILON *
@ -326,18 +350,24 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(ProxyShape* collisi
// 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, collisionShape1,
transform1, collisionShape2, transform2,
v, contactInfo);
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* collisionShape) {
bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) {
Vector3 suppA; // Support point of object A
Vector3 w; // Support point of Minkowski difference A-B
decimal prevDistSquare;
assert(proxyShape->getCollisionShape()->isConvex());
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);
@ -353,7 +383,7 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* collis
do {
// Compute the support points for original objects (without margins) A and B
suppA = collisionShape->getLocalSupportPointWithoutMargin(-v);
suppA = shape->getLocalSupportPointWithoutMargin(-v, shapeCachedCollisionData);
// Compute the support point for the Minkowski difference A-B
w = suppA - suppB;
@ -394,7 +424,13 @@ bool GJKAlgorithm::testPointInside(const Vector3& localPoint, ProxyShape* collis
// 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".
bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* collisionShape, RaycastInfo& raycastInfo) {
bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo) {
assert(proxyShape->getCollisionShape()->isConvex());
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
@ -402,11 +438,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* collisionShape, RaycastIn
const decimal epsilon = decimal(0.0001);
// Convert the ray origin and direction into the local-space of the collision shape
const Transform localToWorldTransform = collisionShape->getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
Vector3 point1 = worldToLocalTransform * ray.point1;
Vector3 point2 = worldToLocalTransform * ray.point2;
Vector3 rayDirection = point2 - point1;
Vector3 rayDirection = ray.point2 - ray.point1;
// If the points of the segment are two close, return no hit
if (rayDirection.lengthSquare() < machineEpsilonSquare) return false;
@ -418,8 +450,8 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* collisionShape, RaycastIn
Vector3 n(decimal(0.0), decimal(0.0), decimal(0.0));
decimal lambda = decimal(0.0);
suppA = point1; // Current lower bound point on the ray (starting at ray's origin)
suppB = collisionShape->getLocalSupportPointWithoutMargin(rayDirection);
suppA = ray.point1; // Current lower bound point on the ray (starting at ray's origin)
suppB = shape->getLocalSupportPointWithoutMargin(rayDirection, shapeCachedCollisionData);
Vector3 v = suppA - suppB;
decimal vDotW, vDotR;
decimal distSquare = v.lengthSquare();
@ -429,7 +461,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* collisionShape, RaycastIn
while (distSquare > epsilon && nbIterations < MAX_ITERATIONS_GJK_RAYCAST) {
// Compute the support points
suppB = collisionShape->getLocalSupportPointWithoutMargin(v);
suppB = shape->getLocalSupportPointWithoutMargin(v, shapeCachedCollisionData);
w = suppA - suppB;
vDotW = v.dot(w);
@ -445,7 +477,7 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* collisionShape, RaycastIn
// We have found a better lower bound for the hit point along the ray
lambda = lambda - vDotW / vDotR;
suppA = point1 + lambda * rayDirection;
suppA = ray.point1 + lambda * rayDirection;
w = suppA - suppB;
n = v;
}
@ -481,12 +513,12 @@ bool GJKAlgorithm::raycast(const Ray& ray, ProxyShape* collisionShape, RaycastIn
// A raycast hit has been found, we fill in the raycast info
raycastInfo.hitFraction = lambda;
raycastInfo.worldPoint = localToWorldTransform * pointB;
raycastInfo.body = collisionShape->getBody();
raycastInfo.proxyShape = collisionShape;
raycastInfo.worldPoint = pointB;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
if (n.lengthSquare() >= machineEpsilonSquare) { // The normal vector is valid
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * n.getUnit();
raycastInfo.worldNormal = n;
}
else { // Degenerated normal vector, we return a zero normal vector
raycastInfo.worldNormal = Vector3(decimal(0), decimal(0), decimal(0));

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

@ -29,7 +29,7 @@
// Libraries
#include "collision/narrowphase/NarrowPhaseAlgorithm.h"
#include "constraint/ContactPoint.h"
#include "collision/shapes/CollisionShape.h"
#include "collision/shapes/ConvexShape.h"
#include "collision/narrowphase/EPA/EPAAlgorithm.h"
@ -75,33 +75,46 @@ class GJKAlgorithm : public NarrowPhaseAlgorithm {
GJKAlgorithm& operator=(const GJKAlgorithm& algorithm);
/// Compute the penetration depth for enlarged objects.
bool computePenetrationDepthForEnlargedObjects(ProxyShape* collisionShape1,
void computePenetrationDepthForEnlargedObjects(const CollisionShapeInfo& shape1Info,
const Transform& transform1,
ProxyShape* collisionShape2,
const CollisionShapeInfo& shape2Info,
const Transform& transform2,
ContactPointInfo*& contactInfo, Vector3& v);
NarrowPhaseCallback* narrowPhaseCallback,
Vector3& v);
public :
// -------------------- Methods -------------------- //
/// Constructor
GJKAlgorithm(MemoryAllocator& memoryAllocator);
GJKAlgorithm();
/// Destructor
~GJKAlgorithm();
/// Return true and compute a contact info if the two bounding volumes collide.
virtual bool testCollision(ProxyShape* collisionShape1, ProxyShape* collisionShape2,
ContactPointInfo*& contactInfo);
/// Initalize the algorithm
virtual void init(CollisionDetection* collisionDetection,
MemoryAllocator* memoryAllocator);
/// Compute a contact info if the two bounding volumes collide.
virtual void testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback);
/// Use the GJK Algorithm to find if a point is inside a convex collision shape
bool testPointInside(const Vector3& localPoint, ProxyShape* collisionShape);
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* collisionShape, RaycastInfo& raycastInfo);
bool raycast(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo);
};
// Initalize the algorithm
inline void GJKAlgorithm::init(CollisionDetection* collisionDetection,
MemoryAllocator* memoryAllocator) {
NarrowPhaseAlgorithm::init(collisionDetection, memoryAllocator);
mAlgoEPA.init(memoryAllocator);
}
}
#endif

10
src/collision/narrowphase/NarrowPhaseAlgorithm.cpp
View File

@ -30,8 +30,8 @@
using namespace reactphysics3d;
// Constructor
NarrowPhaseAlgorithm::NarrowPhaseAlgorithm(MemoryAllocator& memoryAllocator)
:mMemoryAllocator(memoryAllocator), mCurrentOverlappingPair(NULL) {
NarrowPhaseAlgorithm::NarrowPhaseAlgorithm()
: mMemoryAllocator(NULL), mCurrentOverlappingPair(NULL) {
}
@ -39,3 +39,9 @@ NarrowPhaseAlgorithm::NarrowPhaseAlgorithm(MemoryAllocator& memoryAllocator)
NarrowPhaseAlgorithm::~NarrowPhaseAlgorithm() {
}
// Initalize the algorithm
void NarrowPhaseAlgorithm::init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator) {
mCollisionDetection = collisionDetection;
mMemoryAllocator = memoryAllocator;
}

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

@ -31,17 +31,33 @@
#include "constraint/ContactPoint.h"
#include "memory/MemoryAllocator.h"
#include "engine/OverlappingPair.h"
#include "collision/CollisionShapeInfo.h"
/// Namespace ReactPhysics3D
namespace reactphysics3d {
class CollisionDetection;
// Class NarrowPhaseCallback
/**
* This abstract class is the base class for a narrow-phase collision
* callback class.
*/
class NarrowPhaseCallback {
public:
/// Called by a narrow-phase collision algorithm when a new contact has been found
virtual void notifyContact(OverlappingPair* overlappingPair,
const ContactPointInfo& contactInfo)=0;
};
// Class NarrowPhaseAlgorithm
/**
* This class is an abstract class that represents an algorithm
* used to perform the narrow-phase of a collision detection. The
* goal of the narrow phase algorithm is to compute contact
* informations of a collision between two bodies.
* This abstract class is the base class for a narrow-phase collision
* detection algorithm. The goal of the narrow phase algorithm is to
* compute information about the contact between two proxy shapes.
*/
class NarrowPhaseAlgorithm {
@ -49,8 +65,11 @@ class NarrowPhaseAlgorithm {
// -------------------- Attributes -------------------- //
/// Reference to the memory allocator
MemoryAllocator& mMemoryAllocator;
/// Pointer to the collision detection object
CollisionDetection* mCollisionDetection;
/// Pointer to the memory allocator
MemoryAllocator* mMemoryAllocator;
/// Overlapping pair of the bodies currently tested for collision
OverlappingPair* mCurrentOverlappingPair;
@ -68,17 +87,21 @@ class NarrowPhaseAlgorithm {
// -------------------- Methods -------------------- //
/// Constructor
NarrowPhaseAlgorithm(MemoryAllocator& memoryAllocator);
NarrowPhaseAlgorithm();
/// Destructor
virtual ~NarrowPhaseAlgorithm();
/// Initalize the algorithm
virtual void init(CollisionDetection* collisionDetection, MemoryAllocator* memoryAllocator);
/// Set the current overlapping pair of bodies
void setCurrentOverlappingPair(OverlappingPair* overlappingPair);
/// Return true and compute a contact info if the two bounding volume collide
virtual bool testCollision(ProxyShape* collisionShape1, ProxyShape* collisionShape2,
ContactPointInfo*& contactInfo)=0;
/// 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

35
src/collision/narrowphase/SphereVsSphereAlgorithm.cpp
View File

@ -31,8 +31,7 @@
using namespace reactphysics3d;
// Constructor
SphereVsSphereAlgorithm::SphereVsSphereAlgorithm(MemoryAllocator& memoryAllocator)
:NarrowPhaseAlgorithm(memoryAllocator) {
SphereVsSphereAlgorithm::SphereVsSphereAlgorithm() : NarrowPhaseAlgorithm() {
}
@ -41,21 +40,17 @@ SphereVsSphereAlgorithm::~SphereVsSphereAlgorithm() {
}
bool SphereVsSphereAlgorithm::testCollision(ProxyShape* collisionShape1,
ProxyShape* collisionShape2,
ContactPointInfo*& contactInfo) {
void SphereVsSphereAlgorithm::testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback) {
// Get the sphere collision shapes
const CollisionShape* shape1 = collisionShape1->getCollisionShape();
const CollisionShape* shape2 = collisionShape2->getCollisionShape();
const SphereShape* sphereShape1 = static_cast<const SphereShape*>(shape1);
const SphereShape* sphereShape2 = static_cast<const SphereShape*>(shape2);
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 = collisionShape1->getBody()->getTransform() *
collisionShape1->getLocalToBodyTransform();
const Transform transform2 = collisionShape2->getBody()->getTransform() *
collisionShape2->getLocalToBodyTransform();
const Transform& transform1 = shape1Info.shapeToWorldTransform;
const Transform& transform2 = shape2Info.shapeToWorldTransform;
// Compute the distance between the centers
Vector3 vectorBetweenCenters = transform2.getPosition() - transform1.getPosition();
@ -75,13 +70,11 @@ bool SphereVsSphereAlgorithm::testCollision(ProxyShape* collisionShape1,
decimal penetrationDepth = sumRadius - std::sqrt(squaredDistanceBetweenCenters);
// Create the contact info object
contactInfo = new (mMemoryAllocator.allocate(sizeof(ContactPointInfo)))
ContactPointInfo(collisionShape1, collisionShape2,
vectorBetweenCenters.getUnit(), penetrationDepth,
intersectionOnBody1, intersectionOnBody2);
return true;
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);
}
return false;
}

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

@ -57,14 +57,15 @@ class SphereVsSphereAlgorithm : public NarrowPhaseAlgorithm {
// -------------------- Methods -------------------- //
/// Constructor
SphereVsSphereAlgorithm(MemoryAllocator& memoryAllocator);
SphereVsSphereAlgorithm();
/// Destructor
virtual ~SphereVsSphereAlgorithm();
/// Return true and compute a contact info if the two bounding volume collide
virtual bool testCollision(ProxyShape* collisionShape1, ProxyShape* collisionShape2,
ContactPointInfo*& contactInfo);
/// Compute a contact info if the two bounding volume collide
virtual void testCollision(const CollisionShapeInfo& shape1Info,
const CollisionShapeInfo& shape2Info,
NarrowPhaseCallback* narrowPhaseCallback);
};
}

61
src/collision/shapes/AABB.cpp
View File

@ -76,7 +76,7 @@ void AABB::mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2) {
}
// Return true if the current AABB contains the AABB given in parameter
bool AABB::contains(const AABB& aabb) {
bool AABB::contains(const AABB& aabb) const {
bool isInside = true;
isInside = isInside && mMinCoordinates.x <= aabb.mMinCoordinates.x;
@ -89,3 +89,62 @@ bool AABB::contains(const AABB& aabb) {
return isInside;
}
// Create and return an AABB for a triangle
AABB AABB::createAABBForTriangle(const Vector3* trianglePoints) {
Vector3 minCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z);
Vector3 maxCoords(trianglePoints[0].x, trianglePoints[0].y, trianglePoints[0].z);
if (trianglePoints[1].x < minCoords.x) minCoords.x = trianglePoints[1].x;
if (trianglePoints[1].y < minCoords.y) minCoords.y = trianglePoints[1].y;
if (trianglePoints[1].z < minCoords.z) minCoords.z = trianglePoints[1].z;
if (trianglePoints[2].x < minCoords.x) minCoords.x = trianglePoints[2].x;
if (trianglePoints[2].y < minCoords.y) minCoords.y = trianglePoints[2].y;
if (trianglePoints[2].z < minCoords.z) minCoords.z = trianglePoints[2].z;
if (trianglePoints[1].x > maxCoords.x) maxCoords.x = trianglePoints[1].x;
if (trianglePoints[1].y > maxCoords.y) maxCoords.y = trianglePoints[1].y;
if (trianglePoints[1].z > maxCoords.z) maxCoords.z = trianglePoints[1].z;
if (trianglePoints[2].x > maxCoords.x) maxCoords.x = trianglePoints[2].x;
if (trianglePoints[2].y > maxCoords.y) maxCoords.y = trianglePoints[2].y;
if (trianglePoints[2].z > maxCoords.z) maxCoords.z = trianglePoints[2].z;
return AABB(minCoords, maxCoords);
}
// Return true if the ray intersects the AABB
/// This method use the line vs AABB raycasting technique described in
/// Real-time Collision Detection by Christer Ericson.
bool AABB::testRayIntersect(const Ray& ray) const {
const Vector3 point2 = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
const Vector3 e = mMaxCoordinates - mMinCoordinates;
const Vector3 d = point2 - ray.point1;
const Vector3 m = ray.point1 + point2 - mMinCoordinates - mMaxCoordinates;
// Test if the AABB face normals are separating axis
decimal adx = std::abs(d.x);
if (std::abs(m.x) > e.x + adx) return false;
decimal ady = std::abs(d.y);
if (std::abs(m.y) > e.y + ady) return false;
decimal adz = std::abs(d.z);
if (std::abs(m.z) > e.z + adz) return false;
// Add in an epsilon term to counteract arithmetic errors when segment is
// (near) parallel to a coordinate axis (see text for detail)
const decimal epsilon = 0.00001;
adx += epsilon;
ady += epsilon;
adz += epsilon;
// Test if the cross products between face normals and ray direction are
// separating axis
if (std::abs(m.y * d.z - m.z * d.y) > e.y * adz + e.z * ady) return false;
if (std::abs(m.z * d.x - m.x * d.z) > e.x * adz + e.z * adx) return false;
if (std::abs(m.x * d.y - m.y * d.x) > e.x * ady + e.y * adx) return false;
// No separating axis has been found
return true;
}

53
src/collision/shapes/AABB.h
View File

@ -82,6 +82,12 @@ class AABB {
/// Set the maximum coordinates of the AABB
void setMax(const Vector3& max);
/// Return the size of the AABB in the three dimension x, y and z
Vector3 getExtent() const;
/// Inflate each side of the AABB by a given size
void inflate(decimal dx, decimal dy, decimal dz);
/// Return true if the current AABB is overlapping with the AABB in argument
bool testCollision(const AABB& aabb) const;
@ -95,7 +101,19 @@ class AABB {
void mergeTwoAABBs(const AABB& aabb1, const AABB& aabb2);
/// Return true if the current AABB contains the AABB given in parameter
bool contains(const AABB& aabb);
bool contains(const AABB& aabb) const;
/// Return true if a point is inside the AABB
bool contains(const Vector3& point) const;
/// Return true if the AABB of a triangle intersects the AABB
bool testCollisionTriangleAABB(const Vector3* trianglePoints) const;
/// Return true if the ray intersects the AABB
bool testRayIntersect(const Ray& ray) const;
/// Create and return an AABB for a triangle
static AABB createAABBForTriangle(const Vector3* trianglePoints);
/// Assignment operator
AABB& operator=(const AABB& aabb);
@ -130,6 +148,17 @@ inline void AABB::setMax(const Vector3& max) {
mMaxCoordinates = max;
}
// Return the size of the AABB in the three dimension x, y and z
inline Vector3 AABB::getExtent() const {
return mMaxCoordinates - mMinCoordinates;
}
// Inflate each side of the AABB by a given size
inline void AABB::inflate(decimal dx, decimal dy, decimal dz) {
mMaxCoordinates += Vector3(dx, dy, dz);
mMinCoordinates -= Vector3(dx, dy, dz);
}
// Return true if the current AABB is overlapping with the AABB in argument.
/// Two AABBs overlap if they overlap in the three x, y and z axis at the same time
inline bool AABB::testCollision(const AABB& aabb) const {
@ -148,6 +177,28 @@ inline decimal AABB::getVolume() const {
return (diff.x * diff.y * diff.z);
}
// Return true if the AABB of a triangle intersects the AABB
inline bool AABB::testCollisionTriangleAABB(const Vector3* trianglePoints) const {
if (min3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) > mMaxCoordinates.x) return false;
if (min3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) > mMaxCoordinates.y) return false;
if (min3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) > mMaxCoordinates.z) return false;
if (max3(trianglePoints[0].x, trianglePoints[1].x, trianglePoints[2].x) < mMinCoordinates.x) return false;
if (max3(trianglePoints[0].y, trianglePoints[1].y, trianglePoints[2].y) < mMinCoordinates.y) return false;
if (max3(trianglePoints[0].z, trianglePoints[1].z, trianglePoints[2].z) < mMinCoordinates.z) return false;
return true;
}
// Return true if a point is inside the AABB
inline bool AABB::contains(const Vector3& point) const {
return (point.x >= mMinCoordinates.x - MACHINE_EPSILON && point.x <= mMaxCoordinates.x + MACHINE_EPSILON &&
point.y >= mMinCoordinates.y - MACHINE_EPSILON && point.y <= mMaxCoordinates.y + MACHINE_EPSILON &&
point.z >= mMinCoordinates.z - MACHINE_EPSILON && point.z <= mMaxCoordinates.z + MACHINE_EPSILON);
}
// Assignment operator
inline AABB& AABB::operator=(const AABB& aabb) {
if (this != &aabb) {

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

@ -38,18 +38,12 @@ using namespace reactphysics3d;
* @param margin The collision margin (in meters) around the collision shape
*/
BoxShape::BoxShape(const Vector3& extent, decimal margin)
: CollisionShape(BOX, margin), mExtent(extent - Vector3(margin, margin, margin)) {
: ConvexShape(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);
}
// Private copy-constructor
BoxShape::BoxShape(const BoxShape& shape) : CollisionShape(shape), mExtent(shape.mExtent) {
}
// Destructor
BoxShape::~BoxShape() {
@ -75,11 +69,7 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const
// Raycast method with feedback information
bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
const Vector3 point1 = worldToLocalTransform * ray.point1;
const Vector3 point2 = worldToLocalTransform * ray.point2;
Vector3 rayDirection = point2 - point1;
Vector3 rayDirection = ray.point2 - ray.point1;
decimal tMin = DECIMAL_SMALLEST;
decimal tMax = DECIMAL_LARGEST;
Vector3 normalDirection(decimal(0), decimal(0), decimal(0));
@ -92,14 +82,14 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro
if (std::abs(rayDirection[i]) < MACHINE_EPSILON) {
// If the ray's origin is not inside the slab, there is no hit
if (point1[i] > mExtent[i] || point1[i] < -mExtent[i]) return false;
if (ray.point1[i] > mExtent[i] || ray.point1[i] < -mExtent[i]) return false;
}
else {
// Compute the intersection of the ray with the near and far plane of the slab
decimal oneOverD = decimal(1.0) / rayDirection[i];
decimal t1 = (-mExtent[i] - point1[i]) * oneOverD;
decimal t2 = (mExtent[i] - point1[i]) * oneOverD;
decimal t1 = (-mExtent[i] - ray.point1[i]) * oneOverD;
decimal t2 = (mExtent[i] - ray.point1[i]) * oneOverD;
currentNormal[0] = (i == 0) ? -mExtent[i] : decimal(0.0);
currentNormal[1] = (i == 1) ? -mExtent[i] : decimal(0.0);
currentNormal[2] = (i == 2) ? -mExtent[i] : decimal(0.0);
@ -127,17 +117,16 @@ bool BoxShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pro
}
// If tMin is negative, we return no hit
if (tMin < decimal(0.0)) return false;
if (tMin < decimal(0.0) || tMin > ray.maxFraction) return false;
// The ray intersects the three slabs, we compute the hit point
Vector3 localHitPoint = point1 + tMin * rayDirection;
Vector3 localHitPoint = ray.point1 + tMin * rayDirection;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = tMin;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
normalDirection.normalize();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
raycastInfo.worldPoint = localHitPoint;
raycastInfo.worldNormal = normalDirection;
return true;
}

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

@ -28,7 +28,7 @@
// Libraries
#include <cfloat>
#include "CollisionShape.h"
#include "ConvexShape.h"
#include "body/CollisionBody.h"
#include "mathematics/mathematics.h"
@ -50,9 +50,9 @@ namespace reactphysics3d {
* constructor of the box shape. Otherwise, it is recommended to use the
* default margin distance by not using the "margin" parameter in the constructor.
*/
class BoxShape : public CollisionShape {
class BoxShape : public ConvexShape {
private :
protected :
// -------------------- Attributes -------------------- //
@ -67,10 +67,6 @@ class BoxShape : public CollisionShape {
/// Private assignment operator
BoxShape& operator=(const BoxShape& shape);
/// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const;
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const;
@ -81,9 +77,6 @@ class BoxShape : public CollisionShape {
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
/// Allocate and return a copy of the object
virtual BoxShape* clone(void* allocatedMemory) const;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const;
@ -100,21 +93,16 @@ class BoxShape : public CollisionShape {
/// Return the extents of the box
Vector3 getExtent() const;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
/// Test equality between two box shapes
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
};
// Allocate and return a copy of the object
inline BoxShape* BoxShape::clone(void* allocatedMemory) const {
return new (allocatedMemory) BoxShape(*this);
}
// Return the extents of the box
/**
* @return The vector with the three extents of the box shape (in meters)
@ -123,6 +111,14 @@ inline Vector3 BoxShape::getExtent() const {
return mExtent + Vector3(mMargin, mMargin, mMargin);
}
// Set the scaling vector of the collision shape
inline void BoxShape::setLocalScaling(const Vector3& scaling) {
mExtent = (mExtent / mScaling) * scaling;
CollisionShape::setLocalScaling(scaling);
}
// Return the local bounds of the shape in x, y and z directions
/// This method is used to compute the AABB of the box
/**
@ -143,17 +139,6 @@ inline size_t BoxShape::getSizeInBytes() const {
return sizeof(BoxShape);
}
// Return a local support point in a given direction with the object margin
inline Vector3 BoxShape::getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const {
assert(mMargin > 0.0);
return Vector3(direction.x < 0.0 ? -mExtent.x - mMargin : mExtent.x + mMargin,
direction.y < 0.0 ? -mExtent.y - mMargin : mExtent.y + mMargin,
direction.z < 0.0 ? -mExtent.z - mMargin : mExtent.z + mMargin);
}
// Return a local support point in a given direction without the objec margin
inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
@ -163,12 +148,6 @@ inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direct
direction.z < 0.0 ? -mExtent.z : mExtent.z);
}
// Test equality between two box shapes
inline bool BoxShape::isEqualTo(const CollisionShape& otherCollisionShape) const {
const BoxShape& otherShape = static_cast<const BoxShape&>(otherCollisionShape);
return (mExtent == otherShape.mExtent);
}
// Return true if a point is inside the collision shape
inline bool BoxShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
return (localPoint.x < mExtent[0] && localPoint.x > -mExtent[0] &&

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

@ -37,79 +37,16 @@ using namespace reactphysics3d;
* @param height The height of the capsule (in meters)
*/
CapsuleShape::CapsuleShape(decimal radius, decimal height)
: CollisionShape(CAPSULE, radius), mRadius(radius), mHalfHeight(height * decimal(0.5)) {
: ConvexShape(CAPSULE, radius), mHalfHeight(height * decimal(0.5)) {
assert(radius > decimal(0.0));
assert(height > decimal(0.0));
}
// Private copy-constructor
CapsuleShape::CapsuleShape(const CapsuleShape& shape)
: CollisionShape(shape), mRadius(shape.mRadius), mHalfHeight(shape.mHalfHeight) {
}
// Destructor
CapsuleShape::~CapsuleShape() {
}
// Return a local support point in a given direction with the object margin.
/// A capsule is the convex hull of two spheres S1 and S2. The support point in the direction "d"
/// of the convex hull of a set of convex objects is the support point "p" in the set of all
/// support points from all the convex objects with the maximum dot product with the direction "d".
/// 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.
Vector3 CapsuleShape::getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const {
// If the direction vector is not the zero vector
if (direction.lengthSquare() >= MACHINE_EPSILON * MACHINE_EPSILON) {
Vector3 unitDirection = direction.getUnit();
// Support point top sphere
Vector3 centerTopSphere(0, mHalfHeight, 0);
Vector3 topSpherePoint = centerTopSphere + unitDirection * mRadius;
decimal dotProductTop = topSpherePoint.dot(direction);
// Support point bottom sphere
Vector3 centerBottomSphere(0, -mHalfHeight, 0);
Vector3 bottomSpherePoint = centerBottomSphere + unitDirection * mRadius;
decimal dotProductBottom = bottomSpherePoint.dot(direction);
// Return the point with the maximum dot product
if (dotProductTop > dotProductBottom) {
return topSpherePoint;
}
else {
return bottomSpherePoint;
}
}
// If the direction vector is the zero vector we return a point on the
// boundary of the capsule
return Vector3(0, mRadius, 0);
}
// Return a local support point in a given direction without the object margin.
Vector3 CapsuleShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
// If the dot product of the direction and the local Y axis (dotProduct = direction.y)
// is positive
if (direction.y > 0.0) {
// Return the top sphere center point
return Vector3(0, mHalfHeight, 0);
}
else {
// Return the bottom sphere center point
return Vector3(0, -mHalfHeight, 0);
}
}
// Return the local inertia tensor of the capsule
/**
* @param[out] tensor The 3x3 inertia tensor matrix of the shape in local-space
@ -121,13 +58,13 @@ void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) co
// The inertia tensor formula for a capsule can be found in : Game Engine Gems, Volume 1
decimal height = mHalfHeight + mHalfHeight;
decimal radiusSquare = mRadius * mRadius;
decimal radiusSquare = mMargin * mMargin;
decimal heightSquare = height * height;
decimal radiusSquareDouble = radiusSquare + radiusSquare;
decimal factor1 = decimal(2.0) * mRadius / (decimal(4.0) * mRadius + decimal(3.0) * height);
decimal factor2 = decimal(3.0) * height / (decimal(4.0) * mRadius + decimal(3.0) * height);
decimal factor1 = decimal(2.0) * mMargin / (decimal(4.0) * mMargin + decimal(3.0) * height);
decimal factor2 = decimal(3.0) * height / (decimal(4.0) * mMargin + decimal(3.0) * height);
decimal sum1 = decimal(0.4) * radiusSquareDouble;
decimal sum2 = decimal(0.75) * height * mRadius + decimal(0.5) * heightSquare;
decimal sum2 = decimal(0.75) * height * mMargin + decimal(0.5) * heightSquare;
decimal sum3 = decimal(0.25) * radiusSquare + decimal(1.0 / 12.0) * heightSquare;
decimal IxxAndzz = factor1 * mass * (sum1 + sum2) + factor2 * mass * sum3;
decimal Iyy = factor1 * mass * sum1 + factor2 * mass * decimal(0.25) * radiusSquareDouble;
@ -143,7 +80,7 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
const decimal diffYCenterSphere2 = localPoint.y + mHalfHeight;
const decimal xSquare = localPoint.x * localPoint.x;
const decimal zSquare = localPoint.z * localPoint.z;
const decimal squareRadius = mRadius * mRadius;
const decimal squareRadius = mMargin * mMargin;
// Return true if the point is inside the cylinder or one of the two spheres of the capsule
return ((xSquare + zSquare) < squareRadius &&
@ -155,18 +92,13 @@ bool CapsuleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyS
// Raycast method with feedback information
bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
// Transform the ray direction and origin in local-space coordinates
const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
const Vector3 point1 = worldToLocalTransform * ray.point1;
const Vector3 point2 = worldToLocalTransform * ray.point2;
const Vector3 n = point2 - point1;
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 = point1 - p;
Vector3 m = ray.point1 - p;
decimal t;
decimal mDotD = m.dot(d);
@ -174,16 +106,16 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
decimal dDotD = d.dot(d);
// Test if the segment is outside the cylinder
decimal vec1DotD = (point1 - Vector3(decimal(0.0), -mHalfHeight - mRadius, decimal(0.0))).dot(d);
decimal vec1DotD = (ray.point1 - Vector3(decimal(0.0), -mHalfHeight - mMargin, decimal(0.0))).dot(d);
if (vec1DotD < decimal(0.0) && vec1DotD + nDotD < decimal(0.0)) return false;
decimal ddotDExtraCaps = decimal(2.0) * mRadius * d.y;
decimal ddotDExtraCaps = decimal(2.0) * mMargin * d.y;
if (vec1DotD > dDotD + ddotDExtraCaps && vec1DotD + nDotD > dDotD + ddotDExtraCaps) 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 k = m.dot(m) - mMargin * mMargin;
decimal c = dDotD * k - mDotD * mDotD;
// If the ray is parallel to the capsule axis
@ -200,13 +132,13 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
// Check intersection between the ray and the "p" sphere endcap of the capsule
Vector3 hitLocalPoint;
decimal hitFraction;
if (raycastWithSphereEndCap(point1, point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint;
Vector3 normalDirection = (hitLocalPoint - p).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
raycastInfo.worldPoint = hitLocalPoint;
Vector3 normalDirection = hitLocalPoint - p;
raycastInfo.worldNormal = normalDirection;
return true;
}
@ -218,13 +150,13 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
// Check intersection between the ray and the "q" sphere endcap of the capsule
Vector3 hitLocalPoint;
decimal hitFraction;
if (raycastWithSphereEndCap(point1, point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint;
Vector3 normalDirection = (hitLocalPoint - q).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
raycastInfo.worldPoint = hitLocalPoint;
Vector3 normalDirection = hitLocalPoint - q;
raycastInfo.worldNormal = normalDirection;
return true;
}
@ -251,13 +183,13 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
// Check intersection between the ray and the "p" sphere endcap of the capsule
Vector3 hitLocalPoint;
decimal hitFraction;
if (raycastWithSphereEndCap(point1, point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
if (raycastWithSphereEndCap(ray.point1, ray.point2, p, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint;
Vector3 normalDirection = (hitLocalPoint - p).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
raycastInfo.worldPoint = hitLocalPoint;
Vector3 normalDirection = hitLocalPoint - p;
raycastInfo.worldNormal = normalDirection;
return true;
}
@ -269,13 +201,13 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
// Check intersection between the ray and the "q" sphere endcap of the capsule
Vector3 hitLocalPoint;
decimal hitFraction;
if (raycastWithSphereEndCap(point1, point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
if (raycastWithSphereEndCap(ray.point1, ray.point2, q, ray.maxFraction, hitLocalPoint, hitFraction)) {
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldPoint = localToWorldTransform * hitLocalPoint;
Vector3 normalDirection = (hitLocalPoint - q).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
raycastInfo.worldPoint = hitLocalPoint;
Vector3 normalDirection = hitLocalPoint - q;
raycastInfo.worldNormal = normalDirection;
return true;
}
@ -290,15 +222,15 @@ bool CapsuleShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = point1 + t * n;
Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
raycastInfo.worldPoint = localHitPoint;
Vector3 v = localHitPoint - p;
Vector3 w = (v.dot(d) / d.lengthSquare()) * d;
Vector3 normalDirection = (localHitPoint - (p + w)).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
raycastInfo.worldNormal = normalDirection;
return true;
}
@ -309,7 +241,7 @@ bool CapsuleShape::raycastWithSphereEndCap(const Vector3& point1, const Vector3&
Vector3& hitLocalPoint, decimal& hitFraction) const {
const Vector3 m = point1 - sphereCenter;
decimal c = m.dot(m) - mRadius * mRadius;
decimal c = m.dot(m) - mMargin * mMargin;
// If the origin of the ray is inside the sphere, we return no intersection
if (c < decimal(0.0)) return false;

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

@ -27,7 +27,7 @@
#define REACTPHYSICS3D_CAPSULE_SHAPE_H
// Libraries
#include "CollisionShape.h"
#include "ConvexShape.h"
#include "body/CollisionBody.h"
#include "mathematics/mathematics.h"
@ -44,15 +44,12 @@ namespace reactphysics3d {
* and height of the shape. Therefore, no need to specify an object margin for a
* capsule shape.
*/
class CapsuleShape : public CollisionShape {
class CapsuleShape : public ConvexShape {
private :
protected :
// -------------------- Attributes -------------------- //
/// Radius of the two spheres of the capsule
decimal mRadius;
/// Half height of the capsule (height = distance between the centers of the two spheres)
decimal mHalfHeight;
@ -64,10 +61,6 @@ class CapsuleShape : public CollisionShape {
/// Private assignment operator
CapsuleShape& operator=(const CapsuleShape& shape);
/// Return a local support point in a given direction with the object margin.
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const;
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const;
@ -83,9 +76,6 @@ class CapsuleShape : public CollisionShape {
const Vector3& sphereCenter, decimal maxFraction,
Vector3& hitLocalPoint, decimal& hitFraction) const;
/// Allocate and return a copy of the object
virtual CapsuleShape* clone(void* allocatedMemory) const;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const;
@ -105,27 +95,22 @@ class CapsuleShape : public CollisionShape {
/// Return the height of the capsule
decimal getHeight() const;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
/// Test equality between two capsule shapes
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
};
/// Allocate and return a copy of the object
inline CapsuleShape* CapsuleShape::clone(void* allocatedMemory) const {
return new (allocatedMemory) CapsuleShape(*this);
}
// Get the radius of the capsule
/**
* @return The radius of the capsule shape (in meters)
*/
inline decimal CapsuleShape::getRadius() const {
return mRadius;
return mMargin;
}
// Return the height of the capsule
@ -136,6 +121,15 @@ inline decimal CapsuleShape::getHeight() const {
return mHalfHeight + mHalfHeight;
}
// Set the scaling vector of the collision shape
inline void CapsuleShape::setLocalScaling(const Vector3& scaling) {
mHalfHeight = (mHalfHeight / mScaling.y) * scaling.y;
mMargin = (mMargin / mScaling.x) * scaling.x;
CollisionShape::setLocalScaling(scaling);
}
// Return the number of bytes used by the collision shape
inline size_t CapsuleShape::getSizeInBytes() const {
return sizeof(CapsuleShape);
@ -150,20 +144,39 @@ inline size_t CapsuleShape::getSizeInBytes() const {
inline void CapsuleShape::getLocalBounds(Vector3& min, Vector3& max) const {
// Maximum bounds
max.x = mRadius;
max.y = mHalfHeight + mRadius;
max.z = mRadius;
max.x = mMargin;
max.y = mHalfHeight + mMargin;
max.z = mMargin;
// Minimum bounds
min.x = -mRadius;
min.x = -mMargin;
min.y = -max.y;
min.z = min.x;
}
// Test equality between two capsule shapes
inline bool CapsuleShape::isEqualTo(const CollisionShape& otherCollisionShape) const {
const CapsuleShape& otherShape = static_cast<const CapsuleShape&>(otherCollisionShape);
return (mRadius == otherShape.mRadius && mHalfHeight == otherShape.mHalfHeight);
// Return a local support point in a given direction without the object margin.
/// A capsule is the convex hull of two spheres S1 and S2. The support point in the direction "d"
/// of the convex hull of a set of convex objects is the support point "p" in the set of all
/// support points from all the convex objects with the maximum dot product with the direction "d".
/// 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 {
// Support point top sphere
decimal dotProductTop = mHalfHeight * direction.y;
// Support point bottom sphere
decimal dotProductBottom = -mHalfHeight * direction.y;
// Return the point with the maximum dot product
if (dotProductTop > dotProductBottom) {
return Vector3(0, mHalfHeight, 0);
}
else {
return Vector3(0, -mHalfHeight, 0);
}
}
}

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

@ -32,21 +32,13 @@
using namespace reactphysics3d;
// Constructor
CollisionShape::CollisionShape(CollisionShapeType type, decimal margin)
: mType(type), mNbSimilarCreatedShapes(0), mMargin(margin) {
CollisionShape::CollisionShape(CollisionShapeType type) : mType(type), mScaling(1.0, 1.0, 1.0) {
}
// Private copy-constructor
CollisionShape::CollisionShape(const CollisionShape& shape)
: mType(shape.mType), mNbSimilarCreatedShapes(shape.mNbSimilarCreatedShapes),
mMargin(shape.mMargin) {
}
// Destructor
CollisionShape::~CollisionShape() {
assert(mNbSimilarCreatedShapes == 0);
}
// Compute the world-space AABB of the collision shape given a transform

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

@ -40,7 +40,9 @@
namespace reactphysics3d {
/// Type of the collision shape
enum CollisionShapeType {BOX, SPHERE, CONE, CYLINDER, CAPSULE, CONVEX_MESH};
enum CollisionShapeType {TRIANGLE, BOX, SPHERE, CONE, CYLINDER,
CAPSULE, CONVEX_MESH, CONCAVE_MESH, HEIGHTFIELD};
const int NB_COLLISION_SHAPE_TYPES = 9;
// Declarations
class ProxyShape;
@ -60,11 +62,8 @@ class CollisionShape {
/// Type of the collision shape
CollisionShapeType mType;
/// Current number of similar created shapes
uint mNbSimilarCreatedShapes;
/// Margin used for the GJK collision detection algorithm
decimal mMargin;
/// Scaling vector of the collision shape
Vector3 mScaling;
// -------------------- Methods -------------------- //
@ -74,41 +73,21 @@ class CollisionShape {
/// Private assignment operator
CollisionShape& operator=(const CollisionShape& shape);
// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const=0;
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const=0;
/// Return true if a point is inside the collision shape
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;
/// Return the number of similar created shapes
uint getNbSimilarCreatedShapes() const;
/// Allocate and return a copy of the object
virtual CollisionShape* clone(void* allocatedMemory) const=0;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const = 0;
/// Increment the number of similar allocated collision shapes
void incrementNbSimilarCreatedShapes();
/// Decrement the number of similar allocated collision shapes
void decrementNbSimilarCreatedShapes();
public :
// -------------------- Methods -------------------- //
/// Constructor
CollisionShape(CollisionShapeType type, decimal margin);
CollisionShape(CollisionShapeType type);
/// Destructor
virtual ~CollisionShape();
@ -116,23 +95,30 @@ class CollisionShape {
/// Return the type of the collision shapes
CollisionShapeType getType() const;
/// Return the current object margin
decimal getMargin() const;
/// Return true if the collision shape is convex, false if it is concave
virtual bool isConvex() const=0;
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const=0;
/// Return the scaling vector of the collision shape
Vector3 getScaling() const;
/// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
/// 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;
/// Equality operator between two collision shapes.
bool operator==(const CollisionShape& otherCollisionShape) const;
/// Return true if the collision shape type is a convex shape
static bool isConvex(CollisionShapeType shapeType);
/// Test equality between two collision shapes of the same type (same derived classes).
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const=0;
/// Return the maximum number of contact manifolds in an overlapping pair given two shape types
static int computeNbMaxContactManifolds(CollisionShapeType shapeType1,
CollisionShapeType shapeType2);
// -------------------- Friendship -------------------- //
@ -140,9 +126,6 @@ class CollisionShape {
friend class CollisionWorld;
};
// Return the type of the collision shape
/**
* @return The type of the collision shape (box, sphere, cylinder, ...)
@ -151,44 +134,32 @@ inline CollisionShapeType CollisionShape::getType() const {
return mType;
}
// Return the number of similar created shapes
inline uint CollisionShape::getNbSimilarCreatedShapes() const {
return mNbSimilarCreatedShapes;
// Return true if the collision shape type is a convex shape
inline bool CollisionShape::isConvex(CollisionShapeType shapeType) {
return shapeType != CONCAVE_MESH && shapeType != HEIGHTFIELD;
}
// Return the current collision shape margin
/**
* @return The margin (in meters) around the collision shape
*/
inline decimal CollisionShape::getMargin() const {
return mMargin;
// Return the scaling vector of the collision shape
inline Vector3 CollisionShape::getScaling() const {
return mScaling;
}
// Increment the number of similar allocated collision shapes
inline void CollisionShape::incrementNbSimilarCreatedShapes() {
mNbSimilarCreatedShapes++;
// Set the scaling vector of the collision shape
inline void CollisionShape::setLocalScaling(const Vector3& scaling) {
mScaling = scaling;
}
// Decrement the number of similar allocated collision shapes
inline void CollisionShape::decrementNbSimilarCreatedShapes() {
mNbSimilarCreatedShapes--;
}
// Equality operator between two collision shapes.
/// This methods returns true only if the two collision shapes are of the same type and
/// of the same dimensions.
inline bool CollisionShape::operator==(const CollisionShape& otherCollisionShape) const {
// If the two collisions shapes are not of the same type (same derived classes)
// we return false
if (mType != otherCollisionShape.mType) return false;
assert(typeid(*this) == typeid(otherCollisionShape));
if (mMargin != otherCollisionShape.mMargin) return false;
// Check if the two shapes are equal
return otherCollisionShape.isEqualTo(*this);
// 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;
}
}
}

232
src/collision/shapes/ConcaveMeshShape.cpp Normal file
View File

@ -0,0 +1,232 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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 "ConcaveMeshShape.h"
using namespace reactphysics3d;
// Constructor
ConcaveMeshShape::ConcaveMeshShape(TriangleMesh* triangleMesh)
: ConcaveShape(CONCAVE_MESH) {
mTriangleMesh = triangleMesh;
mRaycastTestType = FRONT;
// Insert all the triangles into the dynamic AABB tree
initBVHTree();
}
// Destructor
ConcaveMeshShape::~ConcaveMeshShape() {
}
// Insert all the triangles into the dynamic AABB tree
void ConcaveMeshShape::initBVHTree() {
// TODO : Try to randomly add the triangles into the tree to obtain a better tree
// For each sub-part of the mesh
for (int subPart=0; subPart<mTriangleMesh->getNbSubparts(); subPart++) {
// 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 (int 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 index of the current vertex in the triangle
int vertexIndex;
if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
vertexIndex = ((uint*)vertexIndexPointer)[k];
}
else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
}
// Get the vertices components of the triangle
if (vertexType == TriangleVertexArray::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::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;
}
}
// 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);
}
}
}
// 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 {
// 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();
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;
if (indexType == TriangleVertexArray::INDEX_INTEGER_TYPE) {
vertexIndex = ((uint*)vertexIndexPointer)[k];
}
else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
vertexIndex = ((unsigned short*)vertexIndexPointer)[k];
}
// Get the vertices components of the triangle
if (vertexType == TriangleVertexArray::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::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;
}
}
}
// Use a callback method on all triangles of the concave shape inside a given AABB
void ConcaveMeshShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const {
ConvexTriangleAABBOverlapCallback overlapCallback(callback, *this, mDynamicAABBTree);
// Ask the Dynamic AABB Tree to report all the triangles that are overlapping
// with the AABB of the convex shape.
mDynamicAABBTree.reportAllShapesOverlappingWithAABB(localAABB, overlapCallback);
}
// 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 {
PROFILE("ConcaveMeshShape::raycast()");
// Create the callback object that will compute ray casting against triangles
ConcaveMeshRaycastCallback raycastCallback(mDynamicAABBTree, *this, proxyShape, raycastInfo, ray);
// 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
// in the hit AABBs.
mDynamicAABBTree.raycast(ray, raycastCallback);
raycastCallback.raycastTriangles();
return raycastCallback.getIsHit();
}
// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree
decimal ConcaveMeshRaycastCallback::raycastBroadPhaseShape(int32 nodeId, const Ray& ray) {
// Add the id of the hit AABB node into
mHitAABBNodes.push_back(nodeId);
return ray.maxFraction;
}
// Raycast all collision shapes that have been collected
void ConcaveMeshRaycastCallback::raycastTriangles() {
std::vector<int>::const_iterator it;
decimal smallestHitFraction = mRay.maxFraction;
for (it = mHitAABBNodes.begin(); it != mHitAABBNodes.end(); ++it) {
// Get the node data (triangle index and mesh subpart index)
int32* data = mDynamicAABBTree.getNodeDataInt(*it);
// Get the triangle vertices for this node from the concave mesh shape
Vector3 trianglePoints[3];
mConcaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
// Create a triangle collision shape
decimal margin = mConcaveMeshShape.getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
triangleShape.setRaycastTestType(mConcaveMeshShape.getRaycastTestType());
// Ray casting test against the collision shape
RaycastInfo raycastInfo;
bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape);
// If the ray hit the collision shape
if (isTriangleHit && raycastInfo.hitFraction <= smallestHitFraction) {
assert(raycastInfo.hitFraction >= decimal(0.0));
mRaycastInfo.body = raycastInfo.body;
mRaycastInfo.proxyShape = raycastInfo.proxyShape;
mRaycastInfo.hitFraction = raycastInfo.hitFraction;
mRaycastInfo.worldPoint = raycastInfo.worldPoint;
mRaycastInfo.worldNormal = raycastInfo.worldNormal;
mRaycastInfo.meshSubpart = data[0];
mRaycastInfo.triangleIndex = data[1];
smallestHitFraction = raycastInfo.hitFraction;
mIsHit = true;
}
}
}

235
src/collision/shapes/ConcaveMeshShape.h Normal file
View File

@ -0,0 +1,235 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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_MESH_SHAPE_H
#define REACTPHYSICS3D_CONCAVE_MESH_SHAPE_H
// Libraries
#include "ConcaveShape.h"
#include "collision/broadphase/DynamicAABBTree.h"
#include "collision/TriangleMesh.h"
#include "collision/shapes/TriangleShape.h"
#include "engine/Profiler.h"
namespace reactphysics3d {
class ConcaveMeshShape;
// class ConvexTriangleAABBOverlapCallback
class ConvexTriangleAABBOverlapCallback : public DynamicAABBTreeOverlapCallback {
private:
TriangleCallback& mTriangleTestCallback;
// Reference to the concave mesh shape
const ConcaveMeshShape& mConcaveMeshShape;
// Reference to the Dynamic AABB tree
const DynamicAABBTree& mDynamicAABBTree;
public:
// Constructor
ConvexTriangleAABBOverlapCallback(TriangleCallback& triangleCallback, const ConcaveMeshShape& concaveShape,
const DynamicAABBTree& dynamicAABBTree)
: mTriangleTestCallback(triangleCallback), mConcaveMeshShape(concaveShape), mDynamicAABBTree(dynamicAABBTree) {
}
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
virtual void notifyOverlappingNode(int nodeId);
};
/// Class ConcaveMeshRaycastCallback
class ConcaveMeshRaycastCallback : public DynamicAABBTreeRaycastCallback {
private :
std::vector<int32> mHitAABBNodes;
const DynamicAABBTree& mDynamicAABBTree;
const ConcaveMeshShape& mConcaveMeshShape;
ProxyShape* mProxyShape;
RaycastInfo& mRaycastInfo;
const Ray& mRay;
bool mIsHit;
public:
// Constructor
ConcaveMeshRaycastCallback(const DynamicAABBTree& dynamicAABBTree, const ConcaveMeshShape& concaveMeshShape,
ProxyShape* proxyShape, RaycastInfo& raycastInfo, const Ray& ray)
: mDynamicAABBTree(dynamicAABBTree), mConcaveMeshShape(concaveMeshShape), mProxyShape(proxyShape),
mRaycastInfo(raycastInfo), mRay(ray), mIsHit(false) {
}
/// Collect all the AABB nodes that are hit by the ray in the Dynamic AABB Tree
virtual decimal raycastBroadPhaseShape(int32 nodeId, const Ray& ray);
/// Raycast all collision shapes that have been collected
void raycastTriangles();
/// Return true if a raycast hit has been found
bool getIsHit() const {
return mIsHit;
}
};
// 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
* this shape for a static mesh.
*/
class ConcaveMeshShape : public ConcaveShape {
protected:
// -------------------- Attributes -------------------- //
/// Triangle mesh
TriangleMesh* mTriangleMesh;
/// Dynamic AABB tree to accelerate collision with the triangles
DynamicAABBTree mDynamicAABBTree;
// -------------------- Methods -------------------- //
/// Private copy-constructor
ConcaveMeshShape(const ConcaveMeshShape& shape);
/// Private assignment operator
ConcaveMeshShape& operator=(const ConcaveMeshShape& shape);
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const;
/// Insert all the triangles into the dynamic AABB tree
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;
public:
/// Constructor
ConcaveMeshShape(TriangleMesh* triangleMesh);
/// Destructor
~ConcaveMeshShape();
/// Return the local bounds of the shape in x, y and z directions.
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
/// Use a callback method on all triangles of the concave shape inside a given AABB
virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const;
// ---------- Friendship ----------- //
friend class ConvexTriangleAABBOverlapCallback;
friend class ConcaveMeshRaycastCallback;
};
// Return the number of bytes used by the collision shape
inline size_t ConcaveMeshShape::getSizeInBytes() const {
return sizeof(ConcaveMeshShape);
}
// Return the local bounds of the shape in x, y and z directions.
// This method is used to compute the AABB of the box
/**
* @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 ConcaveMeshShape::getLocalBounds(Vector3& min, Vector3& max) const {
// Get the AABB of the whole tree
AABB treeAABB = mDynamicAABBTree.getRootAABB();
min = treeAABB.getMin();
max = treeAABB.getMax();
}
// Set the local scaling vector of the collision shape
inline void ConcaveMeshShape::setLocalScaling(const Vector3& scaling) {
CollisionShape::setLocalScaling(scaling);
// Reset the Dynamic AABB Tree
mDynamicAABBTree.reset();
// Rebuild Dynamic AABB Tree here
initBVHTree();
}
// Return the local inertia tensor of the 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 ConcaveMeshShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
// Default inertia tensor
// Note that this is not very realistic for a concave triangle mesh.
// However, in most cases, it will only be used static bodies and therefore,
// the inertia tensor is not used.
tensor.setAllValues(mass, 0, 0,
0, mass, 0,
0, 0, mass);
}
// Called when a overlapping node has been found during the call to
// DynamicAABBTree:reportAllShapesOverlappingWithAABB()
inline void ConvexTriangleAABBOverlapCallback::notifyOverlappingNode(int nodeId) {
// Get the node data (triangle index and mesh subpart index)
int32* data = mDynamicAABBTree.getNodeDataInt(nodeId);
// Get the triangle vertices for this node from the concave mesh shape
Vector3 trianglePoints[3];
mConcaveMeshShape.getTriangleVerticesWithIndexPointer(data[0], data[1], trianglePoints);
// Call the callback to test narrow-phase collision with this triangle
mTriangleTestCallback.testTriangle(trianglePoints);
}
}
#endif

43
src/collision/shapes/ConcaveShape.cpp Normal file
View File

@ -0,0 +1,43 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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 "ConcaveShape.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
ConcaveShape::ConcaveShape(CollisionShapeType type)
: CollisionShape(type), mIsSmoothMeshCollisionEnabled(false),
mTriangleMargin(0), mRaycastTestType(FRONT) {
}
// Destructor
ConcaveShape::~ConcaveShape() {
}

158
src/collision/shapes/ConcaveShape.h Normal file
View File

@ -0,0 +1,158 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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_SHAPE_H
#define REACTPHYSICS3D_CONCAVE_SHAPE_H
// Libraries
#include "CollisionShape.h"
#include "TriangleShape.h"
// ReactPhysics3D namespace
namespace reactphysics3d {
// Class TriangleCallback
/**
* This class is used to encapsulate a callback method for
* a single triangle of a ConcaveMesh.
*/
class TriangleCallback {
public:
/// Report a triangle
virtual void testTriangle(const Vector3* trianglePoints)=0;
};
// Class ConcaveShape
/**
* This abstract class represents a concave collision shape associated with a
* body that is used during the narrow-phase collision detection.
*/
class ConcaveShape : public CollisionShape {
protected :
// -------------------- 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;
// -------------------- Methods -------------------- //
/// Private copy-constructor
ConcaveShape(const ConcaveShape& shape);
/// Private assignment operator
ConcaveShape& operator=(const ConcaveShape& shape);
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
public :
// -------------------- Methods -------------------- //
/// Constructor
ConcaveShape(CollisionShapeType type);
/// Destructor
virtual ~ConcaveShape();
/// Return the triangle margin
decimal getTriangleMargin() const;
/// Return the raycast test type (front, back, front-back)
TriangleRaycastSide getRaycastTestType() const;
// Set the raycast test type (front, back, front-back)
void setRaycastTestType(TriangleRaycastSide testType);
/// Return true if the collision shape is convex, false if it is concave
virtual bool isConvex() const;
/// Use a callback method on all triangles of the concave shape inside a given AABB
virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const=0;
/// 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;
}
// Return true if a point is inside the collision shape
inline bool ConcaveShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
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;
}
// Set the raycast test type (front, back, front-back)
/**
* @param testType Raycast test type for the triangle (front, back, front-back)
*/
inline void ConcaveShape::setRaycastTestType(TriangleRaycastSide testType) {
mRaycastTestType = testType;
}
}
#endif

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

@ -38,7 +38,7 @@ using namespace reactphysics3d;
* @param margin Collision margin (in meters) around the collision shape
*/
ConeShape::ConeShape(decimal radius, decimal height, decimal margin)
: CollisionShape(CONE, margin), mRadius(radius), mHalfHeight(height * decimal(0.5)) {
: ConvexShape(CONE, margin), mRadius(radius), mHalfHeight(height * decimal(0.5)) {
assert(mRadius > decimal(0.0));
assert(mHalfHeight > decimal(0.0));
@ -46,35 +46,11 @@ ConeShape::ConeShape(decimal radius, decimal height, decimal margin)
mSinTheta = mRadius / (sqrt(mRadius * mRadius + height * height));
}
// Private copy-constructor
ConeShape::ConeShape(const ConeShape& shape)
: CollisionShape(shape), mRadius(shape.mRadius), mHalfHeight(shape.mHalfHeight),
mSinTheta(shape.mSinTheta){
}
// Destructor
ConeShape::~ConeShape() {
}
// Return a local support point in a given direction with the object margin
Vector3 ConeShape::getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const {
// Compute the support point without the margin
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
// Add the margin to the support point
Vector3 unitVec(0.0, -1.0, 0.0);
if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) {
unitVec = direction.getUnit();
}
supportPoint += unitVec * mMargin;
return supportPoint;
}
// Return a local support point in a given direction without the object margin
Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
@ -106,12 +82,7 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
// http://www.geometrictools.com/Documentation/IntersectionLineCone.pdf
bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
// Transform the ray direction and origin in local-space coordinates
const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
const Vector3 point1 = worldToLocalTransform * ray.point1;
const Vector3 point2 = worldToLocalTransform * ray.point2;
const Vector3 r = point2 - point1;
const Vector3 r = ray.point2 - ray.point1;
const decimal epsilon = decimal(0.00001);
Vector3 V(0, mHalfHeight, 0);
@ -120,7 +91,7 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
decimal heightSquare = decimal(4.0) * mHalfHeight * mHalfHeight;
decimal cosThetaSquare = heightSquare / (heightSquare + mRadius * mRadius);
decimal factor = decimal(1.0) - cosThetaSquare;
Vector3 delta = point1 - V;
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;
@ -166,10 +137,10 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
}
// If the origin of the ray is inside the cone, we return no hit
if (testPointInside(point1, NULL)) return false;
if (testPointInside(ray.point1, NULL)) return false;
localHitPoint[0] = point1 + tHit[0] * r;
localHitPoint[1] = point1 + tHit[1] * r;
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)) {
@ -191,10 +162,10 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
if (r.y > epsilon) {
// Compute the intersection with the base plane of the cone
tHit[2] = (-point1.y - mHalfHeight) / (r.y);
tHit[2] = (-ray.point1.y - mHalfHeight) / (r.y);
// Only keep this intersection if it is inside the cone radius
localHitPoint[2] = point1 + tHit[2] * r;
localHitPoint[2] = ray.point1 + tHit[2] * r;
if ((localHitPoint[2] - centerBase).lengthSquare() > mRadius * mRadius) {
tHit[2] = decimal(-1.0);
@ -236,8 +207,8 @@ bool ConeShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* pr
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint[hitIndex];
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * localNormal[hitIndex];
raycastInfo.worldPoint = localHitPoint[hitIndex];
raycastInfo.worldNormal = localNormal[hitIndex];
return true;
}

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

@ -27,7 +27,7 @@
#define REACTPHYSICS3D_CONE_SHAPE_H
// Libraries
#include "CollisionShape.h"
#include "ConvexShape.h"
#include "body/CollisionBody.h"
#include "mathematics/mathematics.h"
@ -49,9 +49,9 @@ namespace reactphysics3d {
* 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 CollisionShape {
class ConeShape : public ConvexShape {
private :
protected :
// -------------------- Attributes -------------------- //
@ -72,10 +72,6 @@ class ConeShape : public CollisionShape {
/// Private assignment operator
ConeShape& operator=(const ConeShape& shape);
/// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const;
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const;
@ -86,9 +82,6 @@ class ConeShape : public CollisionShape {
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
/// Allocate and return a copy of the object
virtual ConeShape* clone(void* allocatedMemory) const;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const;
@ -108,21 +101,16 @@ class ConeShape : public CollisionShape {
/// Return the height
decimal getHeight() const;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
/// Test equality between two cone shapes
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
};
// Allocate and return a copy of the object
inline ConeShape* ConeShape::clone(void* allocatedMemory) const {
return new (allocatedMemory) ConeShape(*this);
}
// Return the radius
/**
* @return Radius of the cone (in meters)
@ -139,6 +127,15 @@ 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);
@ -176,12 +173,6 @@ inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass
0.0, 0.0, 0.0, diagXZ);
}
// Test equality between two cone shapes
inline bool ConeShape::isEqualTo(const CollisionShape& otherCollisionShape) const {
const ConeShape& otherShape = static_cast<const ConeShape&>(otherCollisionShape);
return (mRadius == otherShape.mRadius && mHalfHeight == otherShape.mHalfHeight);
}
// 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) /

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

@ -30,7 +30,7 @@
using namespace reactphysics3d;
// Constructor to initialize with a array of 3D vertices.
// Constructor to initialize with an array of 3D vertices.
/// This method creates an internal copy of the input vertices.
/**
* @param arrayVertices Array with the vertices of the convex mesh
@ -38,9 +38,8 @@ 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)
: CollisionShape(CONVEX_MESH, margin), mNbVertices(nbVertices), mMinBounds(0, 0, 0),
ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices, int stride, decimal margin)
: ConvexShape(CONVEX_MESH, margin), mNbVertices(nbVertices), mMinBounds(0, 0, 0),
mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) {
assert(nbVertices > 0);
assert(stride > 0);
@ -58,48 +57,91 @@ ConvexMeshShape::ConvexMeshShape(const decimal* arrayVertices, uint nbVertices,
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(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 (int v = 0; v < triangleVertexArray->getNbVertices(); v++) {
// Get the vertices components of the triangle
if (vertexType == TriangleVertexArray::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::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 (int triangleIndex=0; triangleIndex<triangleVertexArray->getNbTriangles(); triangleIndex++) {
void* vertexIndexPointer = (indicesStart + triangleIndex * 3 * indexStride);
uint vertexIndex[3];
// 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::INDEX_INTEGER_TYPE) {
vertexIndex[k] = ((uint*)vertexIndexPointer)[k];
}
else if (indexType == TriangleVertexArray::INDEX_SHORT_TYPE) {
vertexIndex[k] = ((unsigned short*)vertexIndexPointer)[k];
}
}
// 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)
: CollisionShape(CONVEX_MESH, margin), mNbVertices(0), mMinBounds(0, 0, 0),
: ConvexShape(CONVEX_MESH, margin), mNbVertices(0), mMinBounds(0, 0, 0),
mMaxBounds(0, 0, 0), mIsEdgesInformationUsed(false) {
}
// Private copy-constructor
ConvexMeshShape::ConvexMeshShape(const ConvexMeshShape& shape)
: CollisionShape(shape), mVertices(shape.mVertices), mNbVertices(shape.mNbVertices),
mMinBounds(shape.mMinBounds), mMaxBounds(shape.mMaxBounds),
mIsEdgesInformationUsed(shape.mIsEdgesInformationUsed),
mEdgesAdjacencyList(shape.mEdgesAdjacencyList) {
assert(mNbVertices == mVertices.size());
}
// Destructor
ConvexMeshShape::~ConvexMeshShape() {
}
// Return a local support point in a given direction with the object margin
Vector3 ConvexMeshShape::getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const {
// Get the support point without the margin
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
// Get the unit direction vector
Vector3 unitDirection = direction;
if (direction.lengthSquare() < MACHINE_EPSILON * MACHINE_EPSILON) {
unitDirection.setAllValues(1.0, 1.0, 1.0);
}
unitDirection.normalize();
// Add the margin to the support point and return it
return supportPoint + unitDirection * mMargin;
}
// 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
@ -158,7 +200,7 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct
*((int*)(*cachedCollisionData)) = maxVertex;
// Return the support vertex
return mVertices[maxVertex];
return mVertices[maxVertex] * mScaling;
}
else { // If the edges information is not used
@ -181,13 +223,16 @@ Vector3 ConvexMeshShape::getLocalSupportPointWithoutMargin(const Vector3& direct
assert(maxDotProduct >= decimal(0.0));
// Return the vertex with the largest dot product in the support direction
return mVertices[indexMaxDotProduct];
return mVertices[indexMaxDotProduct] * mScaling;
}
}
// Recompute the bounds of the mesh
void ConvexMeshShape::recalculateBounds() {
// TODO : Only works if the local origin is inside the mesh
// => Make it more robust (init with first vertex of mesh instead)
mMinBounds.setToZero();
mMaxBounds.setToZero();
@ -204,38 +249,15 @@ void ConvexMeshShape::recalculateBounds() {
if (mVertices[i].z < mMinBounds.z) mMinBounds.z = mVertices[i].z;
}
// Apply the local scaling factor
mMaxBounds = mMaxBounds * mScaling;
mMinBounds = mMinBounds * mScaling;
// Add the object margin to the bounds
mMaxBounds += Vector3(mMargin, mMargin, mMargin);
mMinBounds -= Vector3(mMargin, mMargin, mMargin);
}
// Test equality between two cone shapes
bool ConvexMeshShape::isEqualTo(const CollisionShape& otherCollisionShape) const {
const ConvexMeshShape& otherShape = static_cast<const ConvexMeshShape&>(otherCollisionShape);
assert(mNbVertices == mVertices.size());
if (mNbVertices != otherShape.mNbVertices) return false;
if (mIsEdgesInformationUsed != otherShape.mIsEdgesInformationUsed) return false;
if (mEdgesAdjacencyList.size() != otherShape.mEdgesAdjacencyList.size()) return false;
// Check that the vertices are the same
for (uint i=0; i<mNbVertices; i++) {
if (mVertices[i] != otherShape.mVertices[i]) return false;
}
// Check that the edges are the same
for (uint i=0; i<mEdgesAdjacencyList.size(); i++) {
assert(otherShape.mEdgesAdjacencyList.count(i) == 1);
if (mEdgesAdjacencyList.at(i) != otherShape.mEdgesAdjacencyList.at(i)) return false;
}
return true;
}
// Raycast method with feedback information
bool ConvexMeshShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
return proxyShape->mBody->mWorld.mCollisionDetection.mNarrowPhaseGJKAlgorithm.raycast(

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

@ -27,9 +27,10 @@
#define REACTPHYSICS3D_CONVEX_MESH_SHAPE_H
// Libraries
#include "CollisionShape.h"
#include "ConvexShape.h"
#include "engine/CollisionWorld.h"
#include "mathematics/mathematics.h"
#include "collision/TriangleMesh.h"
#include "collision/narrowphase/GJK/GJKAlgorithm.h"
#include <vector>
#include <set>
@ -57,9 +58,9 @@ class CollisionWorld;
* with the addEdge() method. Then, you must use the setIsEdgesInformationUsed(true) method
* in order to use the edges information for collision detection.
*/
class ConvexMeshShape : public CollisionShape {
class ConvexMeshShape : public ConvexShape {
private :
protected :
// -------------------- Attributes -------------------- //
@ -93,9 +94,8 @@ class ConvexMeshShape : public CollisionShape {
/// Recompute the bounds of the mesh
void recalculateBounds();
/// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
/// Return a local support point in a given direction without the object margin.
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
@ -107,9 +107,6 @@ class ConvexMeshShape : public CollisionShape {
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
/// Allocate and return a copy of the object
virtual ConvexMeshShape* clone(void* allocatedMemory) const;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const;
@ -117,10 +114,14 @@ class ConvexMeshShape : public CollisionShape {
// -------------------- Methods -------------------- //
/// Constructor to initialize with a array of 3D vertices.
/// 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);
@ -133,9 +134,6 @@ class ConvexMeshShape : public CollisionShape {
/// Return the local inertia tensor of the collision shape.
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
/// Test equality between two collision shapes
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
/// Add a vertex into the convex mesh
void addVertex(const Vector3& vertex);
@ -150,9 +148,10 @@ class ConvexMeshShape : public CollisionShape {
void setIsEdgesInformationUsed(bool isEdgesUsed);
};
// Allocate and return a copy of the object
inline ConvexMeshShape* ConvexMeshShape::clone(void* allocatedMemory) const {
return new (allocatedMemory) ConvexMeshShape(*this);
/// Set the scaling vector of the collision shape
inline void ConvexMeshShape::setLocalScaling(const Vector3& scaling) {
ConvexShape::setLocalScaling(scaling);
recalculateBounds();
}
// Return the number of bytes used by the collision shape
@ -201,12 +200,12 @@ inline void ConvexMeshShape::addVertex(const Vector3& vertex) {
mNbVertices++;
// Update the bounds of the mesh
if (vertex.x > mMaxBounds.x) mMaxBounds.x = vertex.x;
if (vertex.x < mMinBounds.x) mMinBounds.x = vertex.x;
if (vertex.y > mMaxBounds.y) mMaxBounds.y = vertex.y;
if (vertex.y < mMinBounds.y) mMinBounds.y = vertex.y;
if (vertex.z > mMaxBounds.z) mMaxBounds.z = vertex.z;
if (vertex.z < mMinBounds.z) mMinBounds.z = vertex.z;
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.
@ -219,9 +218,6 @@ inline void ConvexMeshShape::addVertex(const Vector3& vertex) {
*/
inline void ConvexMeshShape::addEdge(uint v1, uint v2) {
assert(v1 >= 0);
assert(v2 >= 0);
// 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>()));

62
src/collision/shapes/ConvexShape.cpp Normal file
View File

@ -0,0 +1,62 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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 "ConvexShape.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
ConvexShape::ConvexShape(CollisionShapeType type, decimal margin)
: CollisionShape(type), mMargin(margin) {
}
// Destructor
ConvexShape::~ConvexShape() {
}
// Return a local support point in a given direction with the object margin
Vector3 ConvexShape::getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const {
// Get the support point without margin
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, cachedCollisionData);
if (mMargin != decimal(0.0)) {
// Add the margin to the support point
Vector3 unitVec(0.0, -1.0, 0.0);
if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) {
unitVec = direction.getUnit();
}
supportPoint += unitVec * mMargin;
}
return supportPoint;
}

106
src/collision/shapes/ConvexShape.h Normal file
View File

@ -0,0 +1,106 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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_SHAPE_H
#define REACTPHYSICS3D_CONVEX_SHAPE_H
// Libraries
#include "CollisionShape.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
// Class ConvexShape
/**
* This abstract class represents a convex collision shape associated with a
* body that is used during the narrow-phase collision detection.
*/
class ConvexShape : public CollisionShape {
protected :
// -------------------- Attributes -------------------- //
/// Margin used for the GJK collision detection algorithm
decimal mMargin;
// -------------------- Methods -------------------- //
/// Private copy-constructor
ConvexShape(const ConvexShape& shape);
/// Private assignment operator
ConvexShape& operator=(const ConvexShape& shape);
// Return a local support point in a given direction with the object margin
Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const;
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const=0;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& worldPoint, ProxyShape* proxyShape) const=0;
public :
// -------------------- Methods -------------------- //
/// Constructor
ConvexShape(CollisionShapeType type, decimal margin);
/// Destructor
virtual ~ConvexShape();
/// Return the current object margin
decimal getMargin() const;
/// Return true if the collision shape is convex, false if it is concave
virtual bool isConvex() const;
// -------------------- Friendship -------------------- //
friend class GJKAlgorithm;
friend class EPAAlgorithm;
};
/// Return true if the collision shape is convex, false if it is concave
inline bool ConvexShape::isConvex() const {
return true;
}
// Return the current collision shape margin
/**
* @return The margin (in meters) around the collision shape
*/
inline decimal ConvexShape::getMargin() const {
return mMargin;
}
}
#endif

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

@ -37,40 +37,17 @@ using namespace reactphysics3d;
* @param margin Collision margin (in meters) around the collision shape
*/
CylinderShape::CylinderShape(decimal radius, decimal height, decimal margin)
: CollisionShape(CYLINDER, margin), mRadius(radius),
: ConvexShape(CYLINDER, margin), mRadius(radius),
mHalfHeight(height/decimal(2.0)) {
assert(radius > decimal(0.0));
assert(height > decimal(0.0));
}
// Private copy-constructor
CylinderShape::CylinderShape(const CylinderShape& shape)
: CollisionShape(shape), mRadius(shape.mRadius), mHalfHeight(shape.mHalfHeight) {
}
// Destructor
CylinderShape::~CylinderShape() {
}
// Return a local support point in a given direction with the object margin
Vector3 CylinderShape::getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const {
// Compute the support point without the margin
Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction, NULL);
// Add the margin to the support point
Vector3 unitVec(0.0, 1.0, 0.0);
if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) {
unitVec = direction.getUnit();
}
supportPoint += unitVec * mMargin;
return supportPoint;
}
// Return a local support point in a given direction without the object margin
Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
@ -98,18 +75,13 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio
/// Morgan Kaufmann.
bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
// Transform the ray direction and origin in local-space coordinates
const Transform localToWorldTransform = proxyShape->getLocalToWorldTransform();
const Transform worldToLocalTransform = localToWorldTransform.getInverse();
const Vector3 pointA = worldToLocalTransform * ray.point1;
const Vector3 pointB = worldToLocalTransform * ray.point2;
const Vector3 n = pointB - pointA;
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 = pointA - p;
Vector3 m = ray.point1 - p;
decimal t;
decimal mDotD = m.dot(d);
@ -145,13 +117,13 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = pointA + t * n;
Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(-1), 0);
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
raycastInfo.worldNormal = normalDirection;
return true;
}
@ -164,13 +136,13 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = pointA + t * n;
Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(1.0), 0);
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
raycastInfo.worldNormal = normalDirection;
return true;
}
@ -205,13 +177,13 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = pointA + t * n;
Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(-1.0), 0);
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
raycastInfo.worldNormal = normalDirection;
return true;
}
@ -232,13 +204,13 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = pointA + t * n;
Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
raycastInfo.worldPoint = localHitPoint;
Vector3 normalDirection(0, decimal(1.0), 0);
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
raycastInfo.worldNormal = normalDirection;
return true;
}
@ -250,15 +222,15 @@ bool CylinderShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape
if (t < decimal(0.0) || t > ray.maxFraction) return false;
// Compute the hit information
Vector3 localHitPoint = pointA + t * n;
Vector3 localHitPoint = ray.point1 + t * n;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = localToWorldTransform * localHitPoint;
raycastInfo.worldPoint = localHitPoint;
Vector3 v = localHitPoint - p;
Vector3 w = (v.dot(d) / d.lengthSquare()) * d;
Vector3 normalDirection = (localHitPoint - (p + w)).getUnit();
raycastInfo.worldNormal = localToWorldTransform.getOrientation() * normalDirection;
Vector3 normalDirection = (localHitPoint - (p + w));
raycastInfo.worldNormal = normalDirection;
return true;
}

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

@ -27,7 +27,7 @@
#define REACTPHYSICS3D_CYLINDER_SHAPE_H
// Libraries
#include "CollisionShape.h"
#include "ConvexShape.h"
#include "body/CollisionBody.h"
#include "mathematics/mathematics.h"
@ -49,9 +49,9 @@ namespace reactphysics3d {
* 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 CollisionShape {
class CylinderShape : public ConvexShape {
private :
protected :
// -------------------- Attributes -------------------- //
@ -69,10 +69,6 @@ class CylinderShape : public CollisionShape {
/// Private assignment operator
CylinderShape& operator=(const CylinderShape& shape);
/// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const;
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const;
@ -83,9 +79,6 @@ class CylinderShape : public CollisionShape {
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
/// Allocate and return a copy of the object
virtual CylinderShape* clone(void* allocatedMemory) const;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const;
@ -105,21 +98,16 @@ class CylinderShape : public CollisionShape {
/// Return the height
decimal getHeight() const;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
/// Return the local bounds of the shape in x, y and z directions
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
/// Test equality between two cylinder shapes
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
};
/// Allocate and return a copy of the object
inline CylinderShape* CylinderShape::clone(void* allocatedMemory) const {
return new (allocatedMemory) CylinderShape(*this);
}
// Return the radius
/**
* @return Radius of the cylinder (in meters)
@ -136,6 +124,15 @@ inline decimal CylinderShape::getHeight() const {
return mHalfHeight + mHalfHeight;
}
// 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);
@ -173,12 +170,6 @@ inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal
0.0, 0.0, diag);
}
// Test equality between two cylinder shapes
inline bool CylinderShape::isEqualTo(const CollisionShape& otherCollisionShape) const {
const CylinderShape& otherShape = static_cast<const CylinderShape&>(otherCollisionShape);
return (mRadius == otherShape.mRadius && mHalfHeight == otherShape.mHalfHeight);
}
// 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 &&

239
src/collision/shapes/HeightFieldShape.cpp Normal file
View File

@ -0,0 +1,239 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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 "HeightFieldShape.h"
using namespace reactphysics3d;
// Constructor
/**
* @param nbGridColumns Number of columns in the grid of the height field
* @param nbGridRows Number of rows in the grid of the height field
* @param minHeight Minimum height value of the height field
* @param maxHeight Maximum height value of the height field
* @param heightFieldData Pointer to the first height value data (note that values are shared and not copied)
* @param dataType Data type for the height values (int, float, double)
* @param upAxis Integer representing the up axis direction (0 for x, 1 for y and 2 for z)
* @param integerHeightScale Scaling factor used to scale the height values (only when height values type is integer)
*/
HeightFieldShape::HeightFieldShape(int nbGridColumns, int nbGridRows, decimal minHeight, decimal maxHeight,
const void* heightFieldData, HeightDataType dataType, int upAxis,
decimal integerHeightScale)
: ConcaveShape(HEIGHTFIELD), mNbColumns(nbGridColumns), mNbRows(nbGridRows),
mWidth(nbGridColumns - 1), mLength(nbGridRows - 1), mMinHeight(minHeight),
mMaxHeight(maxHeight), mUpAxis(upAxis), mIntegerHeightScale(integerHeightScale),
mHeightDataType(dataType) {
assert(nbGridColumns >= 2);
assert(nbGridRows >= 2);
assert(mWidth >= 1);
assert(mLength >= 1);
assert(minHeight <= maxHeight);
assert(upAxis == 0 || upAxis == 1 || upAxis == 2);
mHeightFieldData = heightFieldData;
decimal halfHeight = (mMaxHeight - mMinHeight) * decimal(0.5);
assert(halfHeight >= 0);
// Compute the local AABB of the height field
if (mUpAxis == 0) {
mAABB.setMin(Vector3(-halfHeight, -mWidth * decimal(0.5), -mLength * decimal(0.5)));
mAABB.setMax(Vector3(halfHeight, mWidth * decimal(0.5), mLength* decimal(0.5)));
}
else if (mUpAxis == 1) {
mAABB.setMin(Vector3(-mWidth * decimal(0.5), -halfHeight, -mLength * decimal(0.5)));
mAABB.setMax(Vector3(mWidth * decimal(0.5), halfHeight, mLength * decimal(0.5)));
}
else if (mUpAxis == 2) {
mAABB.setMin(Vector3(-mWidth * decimal(0.5), -mLength * decimal(0.5), -halfHeight));
mAABB.setMax(Vector3(mWidth * decimal(0.5), mLength * decimal(0.5), halfHeight));
}
}
// Destructor
HeightFieldShape::~HeightFieldShape() {
}
// Return the local bounds of the shape in x, y and z directions.
// This method is used to compute the AABB of the box
/**
* @param min The minimum bounds of the shape in local-space coordinates
* @param max The maximum bounds of the shape in local-space coordinates
*/
void HeightFieldShape::getLocalBounds(Vector3& min, Vector3& max) const {
min = mAABB.getMin() * mScaling;
max = mAABB.getMax() * mScaling;
}
// Test collision with the triangles of the height field shape. The idea is to use the AABB
// of the body when need to test and see against which triangles of the height-field we need
// to test for collision. We compute the sub-grid points that are inside the other body's AABB
// and then for each rectangle in the sub-grid we generate two triangles that we use to test collision.
void HeightFieldShape::testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const {
// Compute the non-scaled AABB
Vector3 inverseScaling(decimal(1.0) / mScaling.x, decimal(1.0) / mScaling.y, decimal(1.0) / mScaling.z);
AABB aabb(localAABB.getMin() * inverseScaling, localAABB.getMax() * inverseScaling);
// Compute the integer grid coordinates inside the area we need to test for collision
int minGridCoords[3];
int maxGridCoords[3];
computeMinMaxGridCoordinates(minGridCoords, maxGridCoords, aabb);
// Compute the starting and ending coords of the sub-grid according to the up axis
int iMin, iMax, jMin, jMax;
switch(mUpAxis) {
case 0 : iMin = clamp(minGridCoords[1], 0, mNbColumns - 1);
iMax = clamp(maxGridCoords[1], 0, mNbColumns - 1);
jMin = clamp(minGridCoords[2], 0, mNbRows - 1);
jMax = clamp(maxGridCoords[2], 0, mNbRows - 1);
break;
case 1 : iMin = clamp(minGridCoords[0], 0, mNbColumns - 1);
iMax = clamp(maxGridCoords[0], 0, mNbColumns - 1);
jMin = clamp(minGridCoords[2], 0, mNbRows - 1);
jMax = clamp(maxGridCoords[2], 0, mNbRows - 1);
break;
case 2 : iMin = clamp(minGridCoords[0], 0, mNbColumns - 1);
iMax = clamp(maxGridCoords[0], 0, mNbColumns - 1);
jMin = clamp(minGridCoords[1], 0, mNbRows - 1);
jMax = clamp(maxGridCoords[1], 0, mNbRows - 1);
break;
}
assert(iMin >= 0 && iMin < mNbColumns);
assert(iMax >= 0 && iMax < mNbColumns);
assert(jMin >= 0 && jMin < mNbRows);
assert(jMax >= 0 && jMax < mNbRows);
// For each sub-grid points (except the last ones one each dimension)
for (int i = iMin; i < iMax; i++) {
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);
// Generate the first triangle for the current grid rectangle
Vector3 trianglePoints[3] = {p1, p2, p3};
// Test collision against the first triangle
callback.testTriangle(trianglePoints);
// Generate the second triangle for the current grid rectangle
trianglePoints[0] = p3;
trianglePoints[1] = p2;
trianglePoints[2] = p4;
// Test collision against the second triangle
callback.testTriangle(trianglePoints);
}
}
}
// Compute the min/max grid coords corresponding to the intersection of the AABB of the height field and
// the AABB to collide
void HeightFieldShape::computeMinMaxGridCoordinates(int* minCoords, int* maxCoords, const AABB& aabbToCollide) const {
// Clamp the min/max coords of the AABB to collide inside the height field AABB
Vector3 minPoint = Vector3::max(aabbToCollide.getMin(), mAABB.getMin());
minPoint = Vector3::min(minPoint, mAABB.getMax());
Vector3 maxPoint = Vector3::min(aabbToCollide.getMax(), mAABB.getMax());
maxPoint = Vector3::max(maxPoint, mAABB.getMin());
// Translate the min/max points such that the we compute grid points from [0 ... mNbWidthGridPoints]
// and from [0 ... mNbLengthGridPoints] because the AABB coordinates range are [-mWdith/2 ... mWidth/2]
// and [-mLength/2 ... mLength/2]
const Vector3 translateVec = mAABB.getExtent() * decimal(0.5);
minPoint += translateVec;
maxPoint += translateVec;
// Convert the floating min/max coords of the AABB into closest integer
// grid values (note that we use the closest grid coordinate that is out
// of the AABB)
minCoords[0] = computeIntegerGridValue(minPoint.x) - 1;
minCoords[1] = computeIntegerGridValue(minPoint.y) - 1;
minCoords[2] = computeIntegerGridValue(minPoint.z) - 1;
maxCoords[0] = computeIntegerGridValue(maxPoint.x) + 1;
maxCoords[1] = computeIntegerGridValue(maxPoint.y) + 1;
maxCoords[2] = computeIntegerGridValue(maxPoint.z) + 1;
}
// 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 {
// TODO : Implement raycasting without using an AABB for the ray
// but using a dynamic AABB tree or octree instead
PROFILE("HeightFieldShape::raycast()");
TriangleOverlapCallback triangleCallback(ray, proxyShape, raycastInfo, *this);
// Compute the AABB for the ray
const Vector3 rayEnd = ray.point1 + ray.maxFraction * (ray.point2 - ray.point1);
const AABB rayAABB(Vector3::min(ray.point1, rayEnd), Vector3::max(ray.point1, rayEnd));
testAllTriangles(triangleCallback, rayAABB);
return triangleCallback.getIsHit();
}
// Raycast test between a ray and a triangle of the heightfield
void TriangleOverlapCallback::testTriangle(const Vector3* trianglePoints) {
// Create a triangle collision shape
decimal margin = mHeightFieldShape.getTriangleMargin();
TriangleShape triangleShape(trianglePoints[0], trianglePoints[1], trianglePoints[2], margin);
triangleShape.setRaycastTestType(mHeightFieldShape.getRaycastTestType());
// Ray casting test against the collision shape
RaycastInfo raycastInfo;
bool isTriangleHit = triangleShape.raycast(mRay, raycastInfo, mProxyShape);
// If the ray hit the collision shape
if (isTriangleHit && raycastInfo.hitFraction <= mSmallestHitFraction) {
assert(raycastInfo.hitFraction >= decimal(0.0));
mRaycastInfo.body = raycastInfo.body;
mRaycastInfo.proxyShape = raycastInfo.proxyShape;
mRaycastInfo.hitFraction = raycastInfo.hitFraction;
mRaycastInfo.worldPoint = raycastInfo.worldPoint;
mRaycastInfo.worldNormal = raycastInfo.worldNormal;
mRaycastInfo.meshSubpart = -1;
mRaycastInfo.triangleIndex = -1;
mSmallestHitFraction = raycastInfo.hitFraction;
mIsHit = true;
}
}

282
src/collision/shapes/HeightFieldShape.h Normal file
View File

@ -0,0 +1,282 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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_HEIGHTFIELD_SHAPE_H
#define REACTPHYSICS3D_HEIGHTFIELD_SHAPE_H
// Libraries
#include "ConcaveShape.h"
#include "collision/shapes/TriangleShape.h"
#include "engine/Profiler.h"
namespace reactphysics3d {
class HeightFieldShape;
// Class TriangleOverlapCallback
/**
* This class is used for testing AABB and triangle overlap for raycasting
*/
class TriangleOverlapCallback : public TriangleCallback {
protected:
const Ray& mRay;
ProxyShape* mProxyShape;
RaycastInfo& mRaycastInfo;
bool mIsHit;
decimal mSmallestHitFraction;
const HeightFieldShape& mHeightFieldShape;
public:
// Constructor
TriangleOverlapCallback(const Ray& ray, ProxyShape* proxyShape, RaycastInfo& raycastInfo,
const HeightFieldShape& heightFieldShape)
: mRay(ray), mProxyShape(proxyShape), mRaycastInfo(raycastInfo),
mHeightFieldShape (heightFieldShape) {
mIsHit = false;
mSmallestHitFraction = mRay.maxFraction;
}
bool getIsHit() const {return mIsHit;}
/// Raycast test between a ray and a triangle of the heightfield
virtual void testTriangle(const Vector3* trianglePoints);
};
// Class HeightFieldShape
/**
* This class represents a static height field that can be used to represent
* a terrain. The height field is made of a grid with rows and columns with a
* height value at each grid point. Note that the height values are not copied into the shape
* but are shared instead. The height values can be of type integer, float or double.
* When creating a HeightFieldShape, you need to specify the minimum and maximum height value of
* your height field. Note that the HeightFieldShape will be re-centered based on its AABB. It means
* that for instance, if the minimum height value is -200 and the maximum value is 400, the final
* minimum height of the field in the simulation will be -300 and the maximum height will be 300.
*/
class HeightFieldShape : public ConcaveShape {
public:
/// Data type for the height data of the height field
enum HeightDataType {HEIGHT_FLOAT_TYPE, HEIGHT_DOUBLE_TYPE, HEIGHT_INT_TYPE};
protected:
// -------------------- Attributes -------------------- //
/// Number of columns in the grid of the height field
int mNbColumns;
/// Number of rows in the grid of the height field
int mNbRows;
/// Height field width
decimal mWidth;
/// Height field length
decimal mLength;
/// Minimum height of the height field
decimal mMinHeight;
/// Maximum height of the height field
decimal mMaxHeight;
/// Up axis direction (0 => x, 1 => y, 2 => z)
int mUpAxis;
/// Height values scale for height field with integer height values
decimal mIntegerHeightScale;
/// Data type of the height values
HeightDataType mHeightDataType;
/// Array of data with all the height values of the height field
const void* mHeightFieldData;
/// Local AABB of the height field (without scaling)
AABB mAABB;
// -------------------- Methods -------------------- //
/// Private copy-constructor
HeightFieldShape(const HeightFieldShape& shape);
/// Private assignment operator
HeightFieldShape& operator=(const HeightFieldShape& shape);
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const;
/// Insert all the triangles into the dynamic AABB tree
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;
/// Return the vertex (local-coordinates) of the height field at a given (x,y) position
Vector3 getVertexAt(int x, int y) const;
/// Return the height of a given (x,y) point in the height field
decimal getHeightAt(int x, int y) const;
/// Return the closest inside integer grid value of a given floating grid value
int computeIntegerGridValue(decimal value) const;
/// 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;
public:
/// Constructor
HeightFieldShape(int nbWidthGridPoints, int nbLengthGridPoints, decimal minHeight, decimal maxHeight,
const void* heightFieldData, HeightDataType dataType,
int upAxis = 1, decimal integerHeightScale = 1.0f);
/// Destructor
~HeightFieldShape();
/// Return the number of rows in the height field
int getNbRows() const;
/// Return the number of columns in the height field
int getNbColumns() const;
/// Return the type of height value in the height field
HeightDataType getHeightDataType() const;
/// Return the local bounds of the shape in x, y and z directions.
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
/// Use a callback method on all triangles of the concave shape inside a given AABB
virtual void testAllTriangles(TriangleCallback& callback, const AABB& localAABB) const;
// ---------- Friendship ----------- //
friend class ConvexTriangleAABBOverlapCallback;
friend class ConcaveMeshRaycastCallback;
};
// Return the number of rows in the height field
inline int HeightFieldShape::getNbRows() const {
return mNbRows;
}
// Return the number of columns in the height field
inline int HeightFieldShape::getNbColumns() const {
return mNbColumns;
}
// Return the type of height value in the height field
inline HeightFieldShape::HeightDataType HeightFieldShape::getHeightDataType() const {
return mHeightDataType;
}
// Return the number of bytes used by the collision shape
inline size_t HeightFieldShape::getSizeInBytes() const {
return sizeof(HeightFieldShape);
}
// Set the local scaling vector of the collision shape
inline void HeightFieldShape::setLocalScaling(const Vector3& scaling) {
CollisionShape::setLocalScaling(scaling);
}
// Return the vertex (local-coordinates) of the height field at a given (x,y) position
inline Vector3 HeightFieldShape::getVertexAt(int x, int y) const {
// Get the height value
const decimal height = getHeightAt(x, y);
// Height values origin
const decimal heightOrigin = -(mMaxHeight - mMinHeight) * decimal(0.5) - mMinHeight;
Vector3 vertex;
switch (mUpAxis) {
case 0: vertex = Vector3(heightOrigin + height, -mWidth * decimal(0.5) + x, -mLength * decimal(0.5) + y);
break;
case 1: vertex = Vector3(-mWidth * decimal(0.5) + x, heightOrigin + height, -mLength * decimal(0.5) + y);
break;
case 2: vertex = Vector3(-mWidth * decimal(0.5) + x, -mLength * decimal(0.5) + y, heightOrigin + height);
break;
default: assert(false);
}
assert(mAABB.contains(vertex));
return vertex * mScaling;
}
// Return the height of a given (x,y) point in the height field
inline decimal HeightFieldShape::getHeightAt(int x, int y) const {
switch(mHeightDataType) {
case HEIGHT_FLOAT_TYPE : return ((float*)mHeightFieldData)[y * mNbColumns + x];
case HEIGHT_DOUBLE_TYPE : return ((double*)mHeightFieldData)[y * mNbColumns + x];
case HEIGHT_INT_TYPE : return ((int*)mHeightFieldData)[y * mNbColumns + x] * mIntegerHeightScale;
default: assert(false);
}
}
// Return the closest inside integer grid value of a given floating grid value
inline int HeightFieldShape::computeIntegerGridValue(decimal value) const {
return (value < decimal(0.0)) ? value - decimal(0.5) : value + decimal(0.5);
}
// Return the local inertia tensor
/**
* @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 HeightFieldShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
// Default inertia tensor
// Note that this is not very realistic for a concave triangle mesh.
// However, in most cases, it will only be used static bodies and therefore,
// the inertia tensor is not used.
tensor.setAllValues(mass, 0, 0,
0, mass, 0,
0, 0, mass);
}
}
#endif

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

@ -35,16 +35,10 @@ using namespace reactphysics3d;
/**
* @param radius Radius of the sphere (in meters)
*/
SphereShape::SphereShape(decimal radius) : CollisionShape(SPHERE, radius), mRadius(radius) {
SphereShape::SphereShape(decimal radius) : ConvexShape(SPHERE, radius) {
assert(radius > decimal(0.0));
}
// Private copy-constructor
SphereShape::SphereShape(const SphereShape& shape)
: CollisionShape(shape), mRadius(shape.mRadius) {
}
// Destructor
SphereShape::~SphereShape() {
@ -53,11 +47,8 @@ SphereShape::~SphereShape() {
// Raycast method with feedback information
bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const {
// We perform the intersection test in world-space
const Vector3 sphereCenter = proxyShape->getLocalToWorldTransform().getPosition();
const Vector3 m = ray.point1 - sphereCenter;
decimal c = m.dot(m) - mRadius * mRadius;
const Vector3 m = ray.point1;
decimal c = m.dot(m) - mMargin * mMargin;
// If the origin of the ray is inside the sphere, we return no intersection
if (c < decimal(0.0)) return false;
@ -91,7 +82,7 @@ bool SphereShape::raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape*
raycastInfo.proxyShape = proxyShape;
raycastInfo.hitFraction = t;
raycastInfo.worldPoint = ray.point1 + t * rayDirection;
raycastInfo.worldNormal = (raycastInfo.worldPoint - sphereCenter).getUnit();
raycastInfo.worldNormal = raycastInfo.worldPoint;
return true;
}

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

@ -27,7 +27,7 @@
#define REACTPHYSICS3D_SPHERE_SHAPE_H
// Libraries
#include "CollisionShape.h"
#include "ConvexShape.h"
#include "body/CollisionBody.h"
#include "mathematics/mathematics.h"
@ -42,14 +42,12 @@ namespace reactphysics3d {
* radius of the sphere. Therefore, no need to specify an object margin
* for a sphere shape.
*/
class SphereShape : public CollisionShape {
class SphereShape : public ConvexShape {
private :
protected :
// -------------------- Attributes -------------------- //
/// Radius of the sphere
decimal mRadius;
// -------------------- Methods -------------------- //
@ -59,10 +57,6 @@ class SphereShape : public CollisionShape {
/// Private assignment operator
SphereShape& operator=(const SphereShape& shape);
/// Return a local support point in a given direction with the object margin
virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const;
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const;
@ -73,9 +67,6 @@ class SphereShape : public CollisionShape {
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
/// Allocate and return a copy of the object
virtual SphereShape* clone(void* allocatedMemory) const;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const;
@ -92,6 +83,9 @@ class SphereShape : public CollisionShape {
/// Return the radius of the sphere
decimal getRadius() const;
/// Set the scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
/// Return the local bounds of the shape in x, y and z directions.
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
@ -99,23 +93,23 @@ class SphereShape : public CollisionShape {
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
/// Update the AABB of a body using its collision shape
virtual void computeAABB(AABB& aabb, const Transform& transform);
/// Test equality between two sphere shapes
virtual bool isEqualTo(const CollisionShape& otherCollisionShape) const;
virtual void computeAABB(AABB& aabb, const Transform& transform) const;
};
/// Allocate and return a copy of the object
inline SphereShape* SphereShape::clone(void* allocatedMemory) const {
return new (allocatedMemory) SphereShape(*this);
}
// Get the radius of the sphere
/**
* @return Radius of the sphere (in meters)
*/
inline decimal SphereShape::getRadius() const {
return mRadius;
return mMargin;
}
// Set the scaling vector of the collision shape
inline void SphereShape::setLocalScaling(const Vector3& scaling) {
mMargin = (mMargin / mScaling.x) * scaling.x;
CollisionShape::setLocalScaling(scaling);
}
// Return the number of bytes used by the collision shape
@ -123,22 +117,6 @@ inline size_t SphereShape::getSizeInBytes() const {
return sizeof(SphereShape);
}
// Return a local support point in a given direction with the object margin
inline Vector3 SphereShape::getLocalSupportPointWithMargin(const Vector3& direction,
void** cachedCollisionData) const {
// If the direction vector is not the zero vector
if (direction.lengthSquare() >= MACHINE_EPSILON * MACHINE_EPSILON) {
// Return the support point of the sphere in the given direction
return mMargin * direction.getUnit();
}
// If the direction vector is the zero vector we return a point on the
// boundary of the sphere
return Vector3(0, mMargin, 0);
}
// Return a local support point in a given direction without the object margin
inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
@ -156,12 +134,12 @@ inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& dir
inline void SphereShape::getLocalBounds(Vector3& min, Vector3& max) const {
// Maximum bounds
max.x = mRadius;
max.y = mRadius;
max.z = mRadius;
max.x = mMargin;
max.y = mMargin;
max.z = mMargin;
// Minimum bounds
min.x = -mRadius;
min.x = -mMargin;
min.y = min.x;
min.z = min.x;
}
@ -173,7 +151,7 @@ inline void SphereShape::getLocalBounds(Vector3& min, Vector3& max) const {
* @param mass Mass to use to compute the inertia tensor of the collision shape
*/
inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
decimal diag = decimal(0.4) * mass * mRadius * mRadius;
decimal diag = decimal(0.4) * mass * mMargin * mMargin;
tensor.setAllValues(diag, 0.0, 0.0,
0.0, diag, 0.0,
0.0, 0.0, diag);
@ -185,25 +163,19 @@ inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal ma
* computed in world-space coordinates
* @param transform Transform used to compute the AABB of the collision shape
*/
inline void SphereShape::computeAABB(AABB& aabb, const Transform& transform) {
inline void SphereShape::computeAABB(AABB& aabb, const Transform& transform) const {
// Get the local extents in x,y and z direction
Vector3 extents(mRadius, mRadius, mRadius);
Vector3 extents(mMargin, mMargin, mMargin);
// Update the AABB with the new minimum and maximum coordinates
aabb.setMin(transform.getPosition() - extents);
aabb.setMax(transform.getPosition() + extents);
}
// Test equality between two sphere shapes
inline bool SphereShape::isEqualTo(const CollisionShape& otherCollisionShape) const {
const SphereShape& otherShape = static_cast<const SphereShape&>(otherCollisionShape);
return (mRadius == otherShape.mRadius);
}
// Return true if a point is inside the collision shape
inline bool SphereShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
return (localPoint.lengthSquare() < mRadius * mRadius);
return (localPoint.lengthSquare() < mMargin * mMargin);
}
}

127
src/collision/shapes/TriangleShape.cpp Normal file
View File

@ -0,0 +1,127 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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 "TriangleShape.h"
#include "collision/ProxyShape.h"
#include "engine/Profiler.h"
#include "configuration.h"
#include <cassert>
using namespace reactphysics3d;
// Constructor
/**
* @param point1 First point of the triangle
* @param point2 Second point of the triangle
* @param point3 Third point of the triangle
* @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(TRIANGLE, margin) {
mPoints[0] = point1;
mPoints[1] = point2;
mPoints[2] = point3;
mRaycastTestType = FRONT;
}
// Destructor
TriangleShape::~TriangleShape() {
}
// 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 {
PROFILE("TriangleShape::raycast()");
const Vector3 pq = ray.point2 - ray.point1;
const Vector3 pa = mPoints[0] - ray.point1;
const Vector3 pb = mPoints[1] - ray.point1;
const Vector3 pc = mPoints[2] - ray.point1;
// Test if the line PQ is inside the eges BC, CA and AB. We use the triple
// product for this test.
const Vector3 m = pq.cross(pc);
decimal u = pb.dot(m);
if (mRaycastTestType == FRONT) {
if (u < decimal(0.0)) return false;
}
else if (mRaycastTestType == BACK) {
if (u > decimal(0.0)) return false;
}
decimal v = -pa.dot(m);
if (mRaycastTestType == FRONT) {
if (v < decimal(0.0)) return false;
}
else if (mRaycastTestType == BACK) {
if (v > decimal(0.0)) return false;
}
else if (mRaycastTestType == FRONT_AND_BACK) {
if (!sameSign(u, v)) return false;
}
decimal w = pa.dot(pq.cross(pb));
if (mRaycastTestType == FRONT) {
if (w < decimal(0.0)) return false;
}
else if (mRaycastTestType == BACK) {
if (w > decimal(0.0)) return false;
}
else if (mRaycastTestType == FRONT_AND_BACK) {
if (!sameSign(u, w)) return false;
}
// If the line PQ is in the triangle plane (case where u=v=w=0)
if (approxEqual(u, 0) && approxEqual(v, 0) && approxEqual(w, 0)) return false;
// Compute the barycentric coordinates (u, v, w) to determine the
// intersection point R, R = u * a + v * b + w * c
decimal denom = decimal(1.0) / (u + v + w);
u *= denom;
v *= denom;
w *= denom;
// Compute the local hit point using the barycentric coordinates
const Vector3 localHitPoint = u * mPoints[0] + v * mPoints[1] + w * mPoints[2];
const decimal hitFraction = (localHitPoint - ray.point1).length() / pq.length();
if (hitFraction < decimal(0.0) || hitFraction > ray.maxFraction) return false;
Vector3 localHitNormal = (mPoints[1] - mPoints[0]).cross(mPoints[2] - mPoints[0]);
if (localHitNormal.dot(pq) > decimal(0.0)) localHitNormal = -localHitNormal;
raycastInfo.body = proxyShape->getBody();
raycastInfo.proxyShape = proxyShape;
raycastInfo.worldPoint = localHitPoint;
raycastInfo.hitFraction = hitFraction;
raycastInfo.worldNormal = localHitNormal;
return true;
}

224
src/collision/shapes/TriangleShape.h Normal file
View File

@ -0,0 +1,224 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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_SHAPE_H
#define REACTPHYSICS3D_TRIANGLE_SHAPE_H
// Libraries
#include "mathematics/mathematics.h"
#include "ConvexShape.h"
/// ReactPhysics3D namespace
namespace reactphysics3d {
/// Raycast test side for the triangle
enum TriangleRaycastSide {
/// Raycast against front triangle
FRONT,
/// Raycast against back triangle
BACK,
/// Raycast against front and back triangle
FRONT_AND_BACK
};
// Class TriangleShape
/**
* This class represents a triangle collision shape that is centered
* at the origin and defined three points.
*/
class TriangleShape : public ConvexShape {
protected:
// -------------------- Attribute -------------------- //
/// Three points of the triangle
Vector3 mPoints[3];
/// Raycast test type for the triangle (front, back, front-back)
TriangleRaycastSide mRaycastTestType;
// -------------------- Methods -------------------- //
/// Private copy-constructor
TriangleShape(const TriangleShape& shape);
/// Private assignment operator
TriangleShape& operator=(const TriangleShape& shape);
/// Return a local support point in a given direction without the object margin
virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const;
/// Return true if a point is inside the collision shape
virtual bool testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const;
/// Raycast method with feedback information
virtual bool raycast(const Ray& ray, RaycastInfo& raycastInfo, ProxyShape* proxyShape) const;
/// Return the number of bytes used by the collision shape
virtual size_t getSizeInBytes() const;
public:
// -------------------- Methods -------------------- //
/// Constructor
TriangleShape(const Vector3& point1, const Vector3& point2, const Vector3& point3,
decimal margin = OBJECT_MARGIN);
/// Destructor
virtual ~TriangleShape();
/// Return the local bounds of the shape in x, y and z directions.
virtual void getLocalBounds(Vector3& min, Vector3& max) const;
/// Set the local scaling vector of the collision shape
virtual void setLocalScaling(const Vector3& scaling);
/// Return the local inertia tensor of the collision shape
virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const;
/// Update the AABB of a body using its collision shape
virtual void computeAABB(AABB& aabb, const Transform& transform) const;
/// Return the raycast test type (front, back, front-back)
TriangleRaycastSide getRaycastTestType() const;
// 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;
// ---------- Friendship ---------- //
friend class ConcaveMeshRaycastCallback;
friend class TriangleOverlapCallback;
};
// Return the number of bytes used by the collision shape
inline size_t TriangleShape::getSizeInBytes() const {
return sizeof(TriangleShape);
}
// Return a local support point in a given direction without the object margin
inline Vector3 TriangleShape::getLocalSupportPointWithoutMargin(const Vector3& direction,
void** cachedCollisionData) const {
Vector3 dotProducts(direction.dot(mPoints[0]), direction.dot(mPoints[1]), direction.dot(mPoints[2]));
return mPoints[dotProducts.getMaxAxis()];
}
// Return the local bounds of the shape in x, y and z directions.
// This method is used to compute the AABB of the box
/**
* @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 TriangleShape::getLocalBounds(Vector3& min, Vector3& max) const {
const Vector3 xAxis(mPoints[0].x, mPoints[1].x, mPoints[2].x);
const Vector3 yAxis(mPoints[0].y, mPoints[1].y, mPoints[2].y);
const Vector3 zAxis(mPoints[0].z, mPoints[1].z, mPoints[2].z);
min.setAllValues(xAxis.getMinValue(), yAxis.getMinValue(), zAxis.getMinValue());
max.setAllValues(xAxis.getMaxValue(), yAxis.getMaxValue(), zAxis.getMaxValue());
min -= Vector3(mMargin, mMargin, mMargin);
max += Vector3(mMargin, mMargin, mMargin);
}
// Set the local scaling vector of the collision shape
inline void TriangleShape::setLocalScaling(const Vector3& scaling) {
mPoints[0] = (mPoints[0] / mScaling) * scaling;
mPoints[1] = (mPoints[1] / mScaling) * scaling;
mPoints[2] = (mPoints[2] / mScaling) * scaling;
CollisionShape::setLocalScaling(scaling);
}
// Return the local inertia tensor of the triangle 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 TriangleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const {
tensor.setToZero();
}
// Update the AABB of a body using its collision shape
/**
* @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
*/
inline void TriangleShape::computeAABB(AABB& aabb, const Transform& transform) const {
const Vector3 worldPoint1 = transform * mPoints[0];
const Vector3 worldPoint2 = transform * mPoints[1];
const Vector3 worldPoint3 = transform * mPoints[2];
const Vector3 xAxis(worldPoint1.x, worldPoint2.x, worldPoint3.x);
const Vector3 yAxis(worldPoint1.y, worldPoint2.y, worldPoint3.y);
const Vector3 zAxis(worldPoint1.z, worldPoint2.z, worldPoint3.z);
aabb.setMin(Vector3(xAxis.getMinValue(), yAxis.getMinValue(), zAxis.getMinValue()));
aabb.setMax(Vector3(xAxis.getMaxValue(), yAxis.getMaxValue(), zAxis.getMaxValue()));
}
// Return true if a point is inside the collision shape
inline bool TriangleShape::testPointInside(const Vector3& localPoint, ProxyShape* proxyShape) const {
return false;
}
// Return the raycast test type (front, back, front-back)
inline TriangleRaycastSide TriangleShape::getRaycastTestType() const {
return mRaycastTestType;
}
// Set the raycast test type (front, back, front-back)
/**
* @param testType Raycast test type for the triangle (front, back, front-back)
*/
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];
}
}
#endif

13
src/configuration.h
View File

@ -51,6 +51,11 @@ typedef long unsigned int luint;
typedef luint bodyindex;
typedef std::pair<bodyindex, bodyindex> bodyindexpair;
typedef signed short int16;
typedef signed int int32;
typedef unsigned short uint16;
typedef unsigned int uint32;
// ------------------- Enumerations ------------------- //
/// Position correction technique used in the constraint solver (for joints).
@ -128,6 +133,14 @@ const decimal DYNAMIC_TREE_AABB_GAP = decimal(0.1);
/// followin constant with the linear velocity and the elapsed time between two frames.
const decimal DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = decimal(1.7);
/// Maximum number of contact manifolds in an overlapping pair that involves two
/// convex collision shapes.
const int NB_MAX_CONTACT_MANIFOLDS_CONVEX_SHAPE = 1;
/// Maximum number of contact manifolds in an overlapping pair that involves at
/// least one concave collision shape.
const int NB_MAX_CONTACT_MANIFOLDS_CONCAVE_SHAPE = 3;
}
#endif

39
src/constraint/ContactPoint.h
View File

@ -28,6 +28,7 @@
// Libraries
#include "body/CollisionBody.h"
#include "collision/CollisionShapeInfo.h"
#include "configuration.h"
#include "mathematics/mathematics.h"
#include "configuration.h"
@ -48,42 +49,42 @@ struct ContactPointInfo {
// -------------------- Methods -------------------- //
/// Private copy-constructor
ContactPointInfo(const ContactPointInfo& contactInfo);
/// Private assignment operator
ContactPointInfo& operator=(const ContactPointInfo& contactInfo);
public:
// -------------------- Attributes -------------------- //
/// First proxy collision shape of the contact
/// First proxy shape of the contact
ProxyShape* shape1;
/// Second proxy collision shape of the contact
/// Second proxy shape of the contact
ProxyShape* shape2;
/// Normal vector the the collision contact in world space
const Vector3 normal;
/// 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
const decimal penetrationDepth;
decimal penetrationDepth;
/// Contact point of body 1 in local space of body 1
const Vector3 localPoint1;
Vector3 localPoint1;
/// Contact point of body 2 in local space of body 2
const Vector3 localPoint2;
Vector3 localPoint2;
// -------------------- Methods -------------------- //
/// Constructor
ContactPointInfo(ProxyShape* proxyShape1, ProxyShape* proxyShape2, const Vector3& normal,
decimal penetrationDepth, const Vector3& localPoint1,
const Vector3& localPoint2)
: shape1(proxyShape1), shape2(proxyShape2), normal(normal),
penetrationDepth(penetrationDepth), localPoint1(localPoint1),
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) {
}
@ -106,7 +107,7 @@ class ContactPoint {
/// Second rigid body of the contact
CollisionBody* mBody2;
/// Normal vector of the contact (From body1 toward body2) in world space
/// Normalized normal vector of the contact (from body1 toward body2) in world space
const Vector3 mNormal;
/// Penetration depth

65
src/engine/CollisionWorld.cpp
View File

@ -49,7 +49,6 @@ CollisionWorld::~CollisionWorld() {
destroyCollisionBody(*itToRemove);
}
assert(mCollisionShapes.empty());
assert(mBodies.empty());
}
@ -118,70 +117,6 @@ bodyindex CollisionWorld::computeNextAvailableBodyID() {
return bodyID;
}
// Create a new collision shape in the world.
/// First, this methods checks that the new collision shape does not exist yet in the
/// world. If it already exists, we do not allocate memory for a new one but instead
/// we reuse the existing one. The goal is to only allocate memory for a single
/// collision shape if this one is used for several bodies in the world. To allocate
/// memory for a new collision shape, we use the memory allocator.
CollisionShape* CollisionWorld::createCollisionShape(const CollisionShape& collisionShape) {
// Check if there is already a similar collision shape in the world
std::list<CollisionShape*>::iterator it;
for (it = mCollisionShapes.begin(); it != mCollisionShapes.end(); ++it) {
if (collisionShape == (*(*it))) {
// Increment the number of similar created shapes
(*it)->incrementNbSimilarCreatedShapes();
// A similar collision shape already exists in the world, so we do not
// create a new one but we simply return a pointer to the existing one
return (*it);
}
}
// A similar collision shape does not already exist in the world, so we create a
// new one and add it to the world
void* allocatedMemory = mMemoryAllocator.allocate(collisionShape.getSizeInBytes());
CollisionShape* newCollisionShape = collisionShape.clone(allocatedMemory);
mCollisionShapes.push_back(newCollisionShape);
newCollisionShape->incrementNbSimilarCreatedShapes();
// Return a pointer to the new collision shape
return newCollisionShape;
}
// Remove a collision shape.
/// First, we check if another body is still using the same collision shape. If so,
/// we keep the allocated collision shape. If it is not the case, we can deallocate
/// the memory associated with the collision shape.
void CollisionWorld::removeCollisionShape(CollisionShape* collisionShape) {
assert(collisionShape->getNbSimilarCreatedShapes() != 0);
// Decrement the number of bodies using the same collision shape
collisionShape->decrementNbSimilarCreatedShapes();
// If no other body is using the collision shape in the world
if (collisionShape->getNbSimilarCreatedShapes() == 0) {
// Remove the shape from the set of shapes in the world
mCollisionShapes.remove(collisionShape);
// Compute the size (in bytes) of the collision shape
size_t nbBytesShape = collisionShape->getSizeInBytes();
// Call the destructor of the collision shape
collisionShape->~CollisionShape();
// Deallocate the memory used by the collision shape
mMemoryAllocator.release(collisionShape, nbBytesShape);
}
}
// Reset all the contact manifolds linked list of each body
void CollisionWorld::resetContactManifoldListsOfBodies() {

23
src/engine/CollisionWorld.h
View File

@ -66,9 +66,6 @@ class CollisionWorld {
/// All the bodies (rigid and soft) of the world
std::set<CollisionBody*> mBodies;
/// All the collision shapes of the world
std::list<CollisionShape*> mCollisionShapes;
/// Current body ID
bodyindex mCurrentBodyID;
@ -92,12 +89,6 @@ class CollisionWorld {
/// Return the next available body ID
bodyindex computeNextAvailableBodyID();
/// Remove a collision shape.
void removeCollisionShape(CollisionShape* collisionShape);
/// Create a new collision shape in the world.
CollisionShape* createCollisionShape(const CollisionShape& collisionShape);
/// Reset all the contact manifolds linked list of each body
void resetContactManifoldListsOfBodies();
@ -123,6 +114,9 @@ class CollisionWorld {
/// Destroy a collision body
void destroyCollisionBody(CollisionBody* collisionBody);
/// Set the collision dispatch configuration
void setCollisionDispatch(CollisionDispatch* collisionDispatch);
/// Ray cast method
void raycast(const Ray& ray, RaycastCallback* raycastCallback,
unsigned short raycastWithCategoryMaskBits = 0xFFFF) const;
@ -182,6 +176,17 @@ inline std::set<CollisionBody*>::iterator CollisionWorld::getBodiesEndIterator()
return mBodies.end();
}
// Set the collision dispatch configuration
/// This can be used to replace default collision detection algorithms by your
/// custom algorithm for instance.
/**
* @param CollisionDispatch Pointer to a collision dispatch object describing
* which collision detection algorithm to use for two given collision shapes
*/
inline void CollisionWorld::setCollisionDispatch(CollisionDispatch* collisionDispatch) {
mCollisionDetection.setCollisionDispatch(collisionDispatch);
}
// Ray cast method
/**
* @param ray Ray to use for raycasting

2
src/engine/ContactSolver.h
View File

@ -30,7 +30,7 @@
#include "constraint/ContactPoint.h"
#include "configuration.h"
#include "constraint/Joint.h"
#include "ContactManifold.h"
#include "collision/ContactManifold.h"
#include "Island.h"
#include "Impulse.h"
#include <map>

13
src/engine/DynamicsWorld.cpp
View File

@ -719,6 +719,8 @@ void DynamicsWorld::computeIslands() {
ContactManifold* contactManifold = contactElement->contactManifold;
assert(contactManifold->getNbContactPoints() > 0);
// Check if the current contact manifold has already been added into an island
if (contactManifold->isAlreadyInIsland()) continue;
@ -984,8 +986,15 @@ std::vector<const ContactManifold*> DynamicsWorld::getContactsList() const {
OverlappingPair* pair = it->second;
// Get the contact manifold
contactManifolds.push_back(pair->getContactManifold());
// For each contact manifold of the pair
const ContactManifoldSet& manifoldSet = pair->getContactManifoldSet();
for (int i=0; i<manifoldSet.getNbContactManifolds(); i++) {
ContactManifold* manifold = manifoldSet.getContactManifold(i);
// Get the contact manifold
contactManifolds.push_back(manifold);
}
}
// Return all the contact manifold

2
src/engine/Island.h
View File

@ -30,7 +30,7 @@
#include "memory/MemoryAllocator.h"
#include "body/RigidBody.h"
#include "constraint/Joint.h"
#include "ContactManifold.h"
#include "collision/ContactManifold.h"
namespace reactphysics3d {

5
src/engine/OverlappingPair.cpp
View File

@ -31,9 +31,8 @@ using namespace reactphysics3d;
// Constructor
OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator)
: mShape1(shape1), mShape2(shape2),
mContactManifold(shape1, shape2, memoryAllocator),
int nbMaxContactManifolds, MemoryAllocator& memoryAllocator)
: mContactManifoldSet(shape1, shape2, memoryAllocator, nbMaxContactManifolds),
mCachedSeparatingAxis(1.0, 1.0, 1.0) {
}

36
src/engine/OverlappingPair.h
View File

@ -27,7 +27,7 @@
#define REACTPHYSICS3D_OVERLAPPING_PAIR_H
// Libraries
#include "ContactManifold.h"
#include "collision/ContactManifoldSet.h"
#include "collision/ProxyShape.h"
#include "collision/shapes/CollisionShape.h"
@ -51,14 +51,8 @@ class OverlappingPair {
// -------------------- Attributes -------------------- //
/// Pointer to the first proxy collision shape
ProxyShape* mShape1;
/// Pointer to the second proxy collision shape
ProxyShape* mShape2;
/// Persistent contact manifold
ContactManifold mContactManifold;
/// Set of persistent contact manifolds
ContactManifoldSet mContactManifoldSet;
/// Cached previous separating axis
Vector3 mCachedSeparatingAxis;
@ -76,7 +70,8 @@ class OverlappingPair {
// -------------------- Methods -------------------- //
/// Constructor
OverlappingPair(ProxyShape* shape1, ProxyShape* shape2, MemoryAllocator& memoryAllocator);
OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
int nbMaxContactManifolds, MemoryAllocator& memoryAllocator);
/// Destructor
~OverlappingPair();
@ -102,8 +97,8 @@ class OverlappingPair {
/// Return the number of contacts in the cache
uint getNbContactPoints() const;
/// Return the contact manifold
ContactManifold* getContactManifold();
/// Return the a reference to the contact manifold set
const ContactManifoldSet& getContactManifoldSet();
/// Clear the contact points of the contact manifold
void clearContactPoints();
@ -121,23 +116,22 @@ class OverlappingPair {
// Return the pointer to first body
inline ProxyShape* OverlappingPair::getShape1() const {
return mShape1;
return mContactManifoldSet.getShape1();
}
// Return the pointer to second body
inline ProxyShape* OverlappingPair::getShape2() const {
return mShape2;
return mContactManifoldSet.getShape2();
}
// Add a contact to the contact manifold
inline void OverlappingPair::addContact(ContactPoint* contact) {
mContactManifold.addContactPoint(contact);
mContactManifoldSet.addContactPoint(contact);
}
// Update the contact manifold
inline void OverlappingPair::update() {
mContactManifold.update(mShape1->getBody()->getTransform() * mShape1->getLocalToBodyTransform(),
mShape2->getBody()->getTransform() *mShape2->getLocalToBodyTransform());
mContactManifoldSet.update();
}
// Return the cached separating axis
@ -153,12 +147,12 @@ inline void OverlappingPair::setCachedSeparatingAxis(const Vector3& axis) {
// Return the number of contact points in the contact manifold
inline uint OverlappingPair::getNbContactPoints() const {
return mContactManifold.getNbContactPoints();
return mContactManifoldSet.getTotalNbContactPoints();
}
// Return the contact manifold
inline ContactManifold* OverlappingPair::getContactManifold() {
return &mContactManifold;
inline const ContactManifoldSet& OverlappingPair::getContactManifoldSet() {
return mContactManifoldSet;
}
// Return the pair of bodies index
@ -187,7 +181,7 @@ inline bodyindexpair OverlappingPair::computeBodiesIndexPair(CollisionBody* body
// Clear the contact points of the contact manifold
inline void OverlappingPair::clearContactPoints() {
mContactManifold.clear();
mContactManifoldSet.clear();
}
}

66
src/engine/Timer.cpp Normal file
View File

@ -0,0 +1,66 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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 "Timer.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
// Constructor
Timer::Timer(double timeStep) : mTimeStep(timeStep), mIsRunning(false) {
assert(timeStep > 0.0);
}
// Destructor
Timer::~Timer() {
}
// Return the current time of the system in seconds
long double Timer::getCurrentSystemTime() {
#if defined(WINDOWS_OS)
LARGE_INTEGER ticksPerSecond;
LARGE_INTEGER ticks;
QueryPerformanceFrequency(&ticksPerSecond);
QueryPerformanceCounter(&ticks);
return (long double(ticks.QuadPart) / long double(ticksPerSecond.QuadPart));
#else
// Initialize the lastUpdateTime with the current time in seconds
timeval timeValue;
gettimeofday(&timeValue, NULL);
return (timeValue.tv_sec + (timeValue.tv_usec / 1000000.0));
#endif
}

200
src/engine/Timer.h Normal file
View File

@ -0,0 +1,200 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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_TIMER_H
#define REACTPHYSICS3D_TIMER_H
// Libraries
#include <stdexcept>
#include <iostream>
#include <ctime>
#include <cassert>
#include "configuration.h"
#if defined(WINDOWS_OS) // For Windows platform
#define NOMINMAX // This is used to avoid definition of max() and min() macros
#include <windows.h>
#else // For Mac OS or Linux platform
#include <sys/time.h>
#endif
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Class Timer
/**
* This class will take care of the time in the physics engine. It
* uses functions that depend on the current platform to get the
* current time.
*/
class Timer {
private :
// -------------------- Attributes -------------------- //
/// Timestep dt of the physics engine (timestep > 0.0)
double mTimeStep;
/// Last time the timer has been updated
long double mLastUpdateTime;
/// Time difference between the two last timer update() calls
long double mDeltaTime;
/// Used to fix the time step and avoid strange time effects
double mAccumulator;
/// True if the timer is running
bool mIsRunning;
// -------------------- Methods -------------------- //
/// Private copy-constructor
Timer(const Timer& timer);
/// Private assignment operator
Timer& operator=(const Timer& timer);
public :
// -------------------- Methods -------------------- //
/// Constructor
Timer(double timeStep);
/// Destructor
virtual ~Timer();
/// Return the timestep of the physics engine
double getTimeStep() const;
/// Set the timestep of the physics engine
void setTimeStep(double timeStep);
/// Return the current time of the physics engine
long double getPhysicsTime() const;
/// Start the timer
void start();
/// Stop the timer
void stop();
/// Return true if the timer is running
bool getIsRunning() const;
/// True if it's possible to take a new step
bool isPossibleToTakeStep() const;
/// Compute the time since the last update() call and add it to the accumulator
void update();
/// Take a new step => update the timer by adding the timeStep value to the current time
void nextStep();
/// Compute the interpolation factor
decimal computeInterpolationFactor();
/// Return the current time of the system in seconds
static long double getCurrentSystemTime();
};
// Return the timestep of the physics engine
inline double Timer::getTimeStep() const {
return mTimeStep;
}
// Set the timestep of the physics engine
inline void Timer::setTimeStep(double timeStep) {
assert(timeStep > 0.0f);
mTimeStep = timeStep;
}
// Return the current time
inline long double Timer::getPhysicsTime() const {
return mLastUpdateTime;
}
// Return if the timer is running
inline bool Timer::getIsRunning() const {
return mIsRunning;
}
// Start the timer
inline void Timer::start() {
if (!mIsRunning) {
// Get the current system time
mLastUpdateTime = getCurrentSystemTime();
mAccumulator = 0.0;
mIsRunning = true;
}
}
// Stop the timer
inline void Timer::stop() {
mIsRunning = false;
}
// True if it's possible to take a new step
inline bool Timer::isPossibleToTakeStep() const {
return (mAccumulator >= mTimeStep);
}
// Take a new step => update the timer by adding the timeStep value to the current time
inline void Timer::nextStep() {
assert(mIsRunning);
// Update the accumulator value
mAccumulator -= mTimeStep;
}
// Compute the interpolation factor
inline decimal Timer::computeInterpolationFactor() {
return (decimal(mAccumulator / mTimeStep));
}
// Compute the time since the last update() call and add it to the accumulator
inline void Timer::update() {
// Get the current system time
long double currentTime = getCurrentSystemTime();
// Compute the delta display time between two display frames
mDeltaTime = currentTime - mLastUpdateTime;
// Update the current display time
mLastUpdateTime = currentTime;
// Update the accumulator value
mAccumulator += mDeltaTime;
}
}
#endif

22
src/mathematics/Vector2.h
View File

@ -132,6 +132,9 @@ struct Vector2 {
/// Overloaded operator
Vector2& operator=(const Vector2& vector);
/// Overloaded less than operator for ordering to be used inside std::set for instance
bool operator<(const Vector2& vector) const;
/// Return a vector taking the minimum components of two vectors
static Vector2 min(const Vector2& vector1, const Vector2& vector2);
@ -145,7 +148,9 @@ struct Vector2 {
friend Vector2 operator-(const Vector2& vector);
friend Vector2 operator*(const Vector2& vector, decimal number);
friend Vector2 operator*(decimal number, const Vector2& vector);
friend Vector2 operator*(const Vector2& vector1, const Vector2& vector2);
friend Vector2 operator/(const Vector2& vector, decimal number);
friend Vector2 operator/(const Vector2& vector1, const Vector2& vector2);
};
// Set the vector to zero
@ -279,12 +284,24 @@ inline Vector2 operator*(const Vector2& vector, decimal number) {
return Vector2(number * vector.x, number * vector.y);
}
// Overloaded operator for multiplication of two vectors
inline Vector2 operator*(const Vector2& vector1, const Vector2& vector2) {
return Vector2(vector1.x * vector2.x, vector1.y * vector2.y);
}
// Overloaded operator for division by a number
inline Vector2 operator/(const Vector2& vector, decimal number) {
assert(number > MACHINE_EPSILON);
return Vector2(vector.x / number, vector.y / number);
}
// Overload operator for division between two vectors
inline Vector2 operator/(const Vector2& vector1, const Vector2& vector2) {
assert(vector2.x > MACHINE_EPSILON);
assert(vector2.y > MACHINE_EPSILON);
return Vector2(vector1.x / vector2.x, vector1.y / vector2.y);
}
// Overloaded operator for multiplication with a number
inline Vector2 operator*(decimal number, const Vector2& vector) {
return vector * number;
@ -299,6 +316,11 @@ inline Vector2& Vector2::operator=(const Vector2& vector) {
return *this;
}
// Overloaded less than operator for ordering to be used inside std::set for instance
inline bool Vector2::operator<(const Vector2& vector) const {
return (x == vector.x ? y < vector.y : x < vector.x);
}
// Return a vector taking the minimum components of two vectors
inline Vector2 Vector2::min(const Vector2& vector1, const Vector2& vector2) {
return Vector2(std::min(vector1.x, vector2.x),

39
src/mathematics/Vector3.h
View File

@ -111,6 +111,12 @@ struct Vector3 {
/// Return the axis with the maximal value
int getMaxAxis() const;
/// Return the minimum value among the three components of a vector
decimal getMinValue() const;
/// Return the maximum value among the three components of a vector
decimal getMaxValue() const;
/// Overloaded operator for the equality condition
bool operator== (const Vector3& vector) const;
@ -138,6 +144,9 @@ struct Vector3 {
/// Overloaded operator
Vector3& operator=(const Vector3& vector);
/// Overloaded less than operator for ordering to be used inside std::set for instance
bool operator<(const Vector3& vector) const;
/// Return a vector taking the minimum components of two vectors
static Vector3 min(const Vector3& vector1, const Vector3& vector2);
@ -151,7 +160,9 @@ struct Vector3 {
friend Vector3 operator-(const Vector3& vector);
friend Vector3 operator*(const Vector3& vector, decimal number);
friend Vector3 operator*(decimal number, const Vector3& vector);
friend Vector3 operator*(const Vector3& vector1, const Vector3& vector2);
friend Vector3 operator/(const Vector3& vector, decimal number);
friend Vector3 operator/(const Vector3& vector1, const Vector3& vector2);
};
// Set the vector to zero
@ -305,11 +316,24 @@ inline Vector3 operator/(const Vector3& vector, decimal number) {
return Vector3(vector.x / number, vector.y / number, vector.z / number);
}
// Overload operator for division between two vectors
inline Vector3 operator/(const Vector3& vector1, const Vector3& vector2) {
assert(vector2.x > MACHINE_EPSILON);
assert(vector2.y > MACHINE_EPSILON);
assert(vector2.z > MACHINE_EPSILON);
return Vector3(vector1.x / vector2.x, vector1.y / vector2.y, vector1.z / vector2.z);
}
// Overloaded operator for multiplication with a number
inline Vector3 operator*(decimal number, const Vector3& vector) {
return vector * number;
}
// Overload operator for multiplication between two vectors
inline Vector3 operator*(const Vector3& vector1, const Vector3& vector2) {
return Vector3(vector1.x * vector2.x, vector1.y * vector2.y, vector1.z * vector2.z);
}
// Assignment operator
inline Vector3& Vector3::operator=(const Vector3& vector) {
if (&vector != this) {
@ -320,6 +344,11 @@ inline Vector3& Vector3::operator=(const Vector3& vector) {
return *this;
}
// Overloaded less than operator for ordering to be used inside std::set for instance
inline bool Vector3::operator<(const Vector3& vector) const {
return (x == vector.x ? (y == vector.y ? z < vector.z : y < vector.y) : x < vector.x);
}
// Return a vector taking the minimum components of two vectors
inline Vector3 Vector3::min(const Vector3& vector1, const Vector3& vector2) {
return Vector3(std::min(vector1.x, vector2.x),
@ -334,6 +363,16 @@ inline Vector3 Vector3::max(const Vector3& vector1, const Vector3& vector2) {
std::max(vector1.z, vector2.z));
}
// Return the minimum value among the three components of a vector
inline decimal Vector3::getMinValue() const {
return std::min(std::min(x, y), z);
}
// Return the maximum value among the three components of a vector
inline decimal Vector3::getMaxValue() const {
return std::max(std::max(x, y), z);
}
}
#endif

51
src/mathematics/mathematics_functions.cpp Normal file
View File

@ -0,0 +1,51 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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 "mathematics_functions.h"
#include "Vector3.h"
using namespace reactphysics3d;
/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c)
/// This method uses the technique described in the book Real-Time collision detection by
/// Christer Ericson.
void reactphysics3d::computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c,
const Vector3& p, decimal& u, decimal& v, decimal& w) {
const Vector3 v0 = b - a;
const Vector3 v1 = c - a;
const Vector3 v2 = p - a;
decimal d00 = v0.dot(v0);
decimal d01 = v0.dot(v1);
decimal d11 = v1.dot(v1);
decimal d20 = v2.dot(v0);
decimal d21 = v2.dot(v1);
decimal denom = d00 * d11 - d01 * d01;
v = (d11 * d20 - d01 * d21) / denom;
w = (d00 * d21 - d01 * d20) / denom;
u = decimal(1.0) - u - w;
}

30
src/mathematics/mathematics_functions.h
View File

@ -30,18 +30,27 @@
#include "configuration.h"
#include "decimal.h"
#include <algorithm>
#include <cassert>
#include <cmath>
/// ReactPhysics3D namespace
namespace reactphysics3d {
struct Vector3;
// ---------- Mathematics functions ---------- //
/// Function to test if two real numbers are (almost) equal
/// We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON]
inline bool approxEqual(decimal a, decimal b, decimal epsilon = MACHINE_EPSILON) {
return (std::fabs(a - b) < epsilon);
}
decimal difference = a - b;
return (difference < epsilon && difference > -epsilon);
/// Function that returns the result of the "value" clamped by
/// two others values "lowerLimit" and "upperLimit"
inline int clamp(int value, int lowerLimit, int upperLimit) {
assert(lowerLimit <= upperLimit);
return std::min(std::max(value, lowerLimit), upperLimit);
}
/// Function that returns the result of the "value" clamped by
@ -51,8 +60,25 @@ inline decimal clamp(decimal value, decimal lowerLimit, decimal upperLimit) {
return std::min(std::max(value, lowerLimit), upperLimit);
}
/// Return the minimum value among three values
inline decimal min3(decimal a, decimal b, decimal c) {
return std::min(std::min(a, b), c);
}
/// Return the maximum value among three values
inline decimal max3(decimal a, decimal b, decimal c) {
return std::max(std::max(a, b), c);
}
/// Return true if two values have the same sign
inline bool sameSign(decimal a, decimal b) {
return a * b >= decimal(0.0);
}
/// Compute the barycentric coordinates u, v, w of a point p inside the triangle (a, b, c)
void computeBarycentricCoordinatesInTriangle(const Vector3& a, const Vector3& b, const Vector3& c,
const Vector3& p, decimal& u, decimal& v, decimal& w);
}
#endif

4
src/reactphysics3d.h
View File

@ -50,9 +50,13 @@
#include "collision/shapes/CylinderShape.h"
#include "collision/shapes/CapsuleShape.h"
#include "collision/shapes/ConvexMeshShape.h"
#include "collision/shapes/ConcaveMeshShape.h"
#include "collision/shapes/HeightFieldShape.h"
#include "collision/shapes/AABB.h"
#include "collision/ProxyShape.h"
#include "collision/RaycastInfo.h"
#include "collision/TriangleMesh.h"
#include "collision/TriangleVertexArray.h"
#include "constraint/BallAndSocketJoint.h"
#include "constraint/SliderJoint.h"
#include "constraint/HingeJoint.h"

6
test/main.cpp
View File

@ -31,9 +31,12 @@
#include "tests/mathematics/TestQuaternion.h"
#include "tests/mathematics/TestMatrix2x2.h"
#include "tests/mathematics/TestMatrix3x3.h"
#include "tests/mathematics/TestMathematicsFunctions.h"
#include "tests/collision/TestPointInside.h"
#include "tests/collision/TestRaycast.h"
#include "tests/collision/TestCollisionWorld.h"
#include "tests/collision/TestAABB.h"
#include "tests/collision/TestDynamicAABBTree.h"
using namespace reactphysics3d;
@ -49,12 +52,15 @@ int main() {
testSuite.addTest(new TestQuaternion("Quaternion"));
testSuite.addTest(new TestMatrix3x3("Matrix3x3"));
testSuite.addTest(new TestMatrix2x2("Matrix2x2"));
testSuite.addTest(new TestMathematicsFunctions("Maths Functions"));
// ---------- Collision Detection tests ---------- //
testSuite.addTest(new TestAABB("AABB"));
testSuite.addTest(new TestPointInside("IsPointInside"));
testSuite.addTest(new TestRaycast("Raycasting"));
testSuite.addTest(new TestCollisionWorld("CollisionWorld"));
testSuite.addTest(new TestDynamicAABBTree("DynamicAABBTree"));
// Run the tests
testSuite.run();

290
test/tests/collision/TestAABB.h Normal file
View File

@ -0,0 +1,290 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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 TEST_AABB_H
#define TEST_AABB_H
// Libraries
#include "reactphysics3d.h"
/// Reactphysics3D namespace
namespace reactphysics3d {
// Class TestAABB
/**
* Unit test for the AABB class.
*/
class TestAABB : public Test {
private :
// ---------- Atributes ---------- //
AABB mAABB1;
AABB mAABB2;
AABB mAABB3;
AABB mAABB4;
public :
// ---------- Methods ---------- //
/// Constructor
TestAABB(const std::string& name) : Test(name) {
mAABB1.setMin(Vector3(-10, -10, -10));
mAABB1.setMax(Vector3(10, 10, 10));
// AABB2 intersect with AABB1
mAABB2.setMin(Vector3(-5, 4, -30));
mAABB2.setMax(Vector3(-2, 20, 30));
// AABB3 contains AABB1
mAABB3.setMin(Vector3(-25, -25, -25));
mAABB3.setMax(Vector3(25, 25, 25));
// AABB4 does not collide with AABB1
mAABB4.setMin(Vector3(-40, -40, -40));
mAABB4.setMax(Vector3(-15, -25, -12));
}
/// Destructor
~TestAABB() {
}
/// Run the tests
void run() {
testBasicMethods();
testMergeMethods();
testIntersection();
}
void testBasicMethods() {
// -------- Test constructors -------- //
AABB aabb1;
AABB aabb2(Vector3(-3, -5, -8), Vector3(65, -1, 56));
Vector3 trianglePoints[] = {
Vector3(-5, 7, 23), Vector3(45, -34, -73), Vector3(-12, 98, 76)
};
AABB aabb3 = AABB::createAABBForTriangle(trianglePoints);
test(aabb1.getMin().x == 0);
test(aabb1.getMin().y == 0);
test(aabb1.getMin().z == 0);
test(aabb1.getMax().x == 0);
test(aabb1.getMax().y == 0);
test(aabb1.getMax().z == 0);
test(aabb2.getMin().x == -3);
test(aabb2.getMin().y == -5);
test(aabb2.getMin().z == -8);
test(aabb2.getMax().x == 65);
test(aabb2.getMax().y == -1);
test(aabb2.getMax().z == 56);
test(aabb3.getMin().x == -12);
test(aabb3.getMin().y == -34);
test(aabb3.getMin().z == -73);
test(aabb3.getMax().x == 45);
test(aabb3.getMax().y == 98);
test(aabb3.getMax().z == 76);
// -------- Test inflate() -------- //
AABB aabbInflate(Vector3(-3, 4, 8), Vector3(-1, 6, 32));
aabbInflate.inflate(1, 2, 3);
test(approxEqual(aabbInflate.getMin().x, -4, 0.00001));
test(approxEqual(aabbInflate.getMin().y, 2, 0.00001));
test(approxEqual(aabbInflate.getMin().z, 5, 0.00001));
test(approxEqual(aabbInflate.getMax().x, 0, 0.00001));
test(approxEqual(aabbInflate.getMax().y, 8, 0.00001));
test(approxEqual(aabbInflate.getMax().z, 35, 0.00001));
// -------- Test getExtent() --------- //
test(approxEqual(mAABB1.getExtent().x, 20));
test(approxEqual(mAABB1.getExtent().y, 20));
test(approxEqual(mAABB1.getExtent().z, 20));
test(approxEqual(mAABB2.getExtent().x, 3));
test(approxEqual(mAABB2.getExtent().y, 16));
test(approxEqual(mAABB2.getExtent().z, 60));
test(approxEqual(mAABB3.getExtent().x, 50));
test(approxEqual(mAABB3.getExtent().y, 50));
test(approxEqual(mAABB3.getExtent().z, 50));
// -------- Test getCenter() -------- //
test(mAABB1.getCenter().x == 0);
test(mAABB1.getCenter().y == 0);
test(mAABB1.getCenter().z == 0);
test(approxEqual(mAABB2.getCenter().x, -3.5));
test(approxEqual(mAABB2.getCenter().y, 12));
test(approxEqual(mAABB2.getCenter().z, 0));
// -------- Test setMin(), setMax(), getMin(), getMax() -------- //
AABB aabb5;
aabb5.setMin(Vector3(-12, 34, 6));
aabb5.setMax(Vector3(-3, 56, 20));
test(aabb5.getMin().x == -12);
test(aabb5.getMin().y == 34);
test(aabb5.getMin().z == 6);
test(aabb5.getMax().x == -3);
test(aabb5.getMax().y == 56);
test(aabb5.getMax().z == 20);
// -------- Test assignment operator -------- //
AABB aabb6;
aabb6 = aabb2;
test(aabb6.getMin().x == -3);
test(aabb6.getMin().y == -5);
test(aabb6.getMin().z == -8);
test(aabb6.getMax().x == 65);
test(aabb6.getMax().y == -1);
test(aabb6.getMax().z == 56);
// -------- Test getVolume() -------- //
test(approxEqual(mAABB1.getVolume(), 8000));
test(approxEqual(mAABB2.getVolume(), 2880));
}
void testMergeMethods() {
AABB aabb1(Vector3(-45, 7, -2), Vector3(23, 8, 1));
AABB aabb2(Vector3(-15, 6, 23), Vector3(-5, 9, 45));
// -------- Test mergeTwoAABBs() -------- //
AABB aabb3;
aabb3.mergeTwoAABBs(aabb1, mAABB1);
test(aabb3.getMin().x == -45);
test(aabb3.getMin().y == -10);
test(aabb3.getMin().z == -10);
test(aabb3.getMax().x == 23);
test(aabb3.getMax().y == 10);
test(aabb3.getMax().z == 10);
AABB aabb4;
aabb4.mergeTwoAABBs(aabb1, aabb2);
test(aabb4.getMin().x == -45);
test(aabb4.getMin().y == 6);
test(aabb4.getMin().z == -2);
test(aabb4.getMax().x == 23);
test(aabb4.getMax().y == 9);
test(aabb4.getMax().z == 45);
// -------- Test mergeWithAABB() -------- //
aabb1.mergeWithAABB(mAABB1);
test(aabb1.getMin().x == -45);
test(aabb1.getMin().y == -10);
test(aabb1.getMin().z == -10);
test(aabb1.getMax().x == 23);
test(aabb1.getMax().y == 10);
test(aabb1.getMax().z == 10);
aabb2.mergeWithAABB(mAABB1);
test(aabb2.getMin().x == -15);
test(aabb2.getMin().y == -10);
test(aabb2.getMin().z == -10);
test(aabb2.getMax().x == 10);
test(aabb2.getMax().y == 10);
test(aabb2.getMax().z == 45);
}
void testIntersection() {
// -------- Test contains(AABB) -------- //
test(!mAABB1.contains(mAABB2));
test(mAABB3.contains(mAABB1));
test(!mAABB1.contains(mAABB3));
test(!mAABB1.contains(mAABB4));
test(!mAABB4.contains(mAABB1));
// -------- Test contains(Vector3) -------- //
test(mAABB1.contains(Vector3(0, 0, 0)));
test(mAABB1.contains(Vector3(-5, 6, 9)));
test(mAABB1.contains(Vector3(-9, -4, -9)));
test(mAABB1.contains(Vector3(9, 4, 7)));
test(!mAABB1.contains(Vector3(-11, -4, -9)));
test(!mAABB1.contains(Vector3(1, 12, -9)));
test(!mAABB1.contains(Vector3(1, 8, -13)));
test(!mAABB1.contains(Vector3(-14, 82, -13)));
// -------- Test testCollision() -------- //
test(mAABB1.testCollision(mAABB2));
test(mAABB2.testCollision(mAABB1));
test(mAABB1.testCollision(mAABB3));
test(mAABB3.testCollision(mAABB1));
test(!mAABB1.testCollision(mAABB4));
test(!mAABB4.testCollision(mAABB1));
// -------- Test testCollisionTriangleAABB() -------- //
AABB aabb(Vector3(100, 100, 100), Vector3(200, 200, 200));
Vector3 trianglePoints[] = {
Vector3(-2, 4, 6), Vector3(20, -34, -73), Vector3(-12, 98, 76)
};
test(mAABB1.testCollisionTriangleAABB(trianglePoints));
test(!aabb.testCollisionTriangleAABB(trianglePoints));
// -------- Test testRayIntersect() -------- //
Ray ray1(Vector3(-20, 4, -7), Vector3(20, 4, -7));
Ray ray2(Vector3(-20, 11, -7), Vector3(20, 11, -7));
Ray ray3(Vector3(0, 15, 0), Vector3(0, -15, 0));
Ray ray4(Vector3(0, -15, 0), Vector3(0, 15, 0));
Ray ray5(Vector3(-3, 4, 8), Vector3(-7, 9, 4));
Ray ray6(Vector3(-4, 6, -100), Vector3(-4, 6, -9));
Ray ray7(Vector3(-4, 6, -100), Vector3(-4, 6, -11), 0.6);
Ray ray8(Vector3(-403, -432, -100), Vector3(134, 643, 23));
test(mAABB1.testRayIntersect(ray1));
test(!mAABB1.testRayIntersect(ray2));
test(mAABB1.testRayIntersect(ray3));
test(mAABB1.testRayIntersect(ray4));
test(mAABB1.testRayIntersect(ray5));
test(mAABB1.testRayIntersect(ray6));
test(!mAABB1.testRayIntersect(ray7));
test(!mAABB1.testRayIntersect(ray8));
}
};
}
#endif

90
test/tests/collision/TestCollisionWorld.h
View File

@ -39,6 +39,8 @@ enum CollisionCategory {
CATEGORY_3 = 0x0004
};
// TODO : Add test for concave shape collision here
// Class
class WorldCollisionCallback : public CollisionCallback
{
@ -112,11 +114,16 @@ class TestCollisionWorld : public Test {
CollisionBody* mSphere2Body;
CollisionBody* mCylinderBody;
// Shapes
ProxyShape* mBoxShape;
ProxyShape* mSphere1Shape;
ProxyShape* mSphere2Shape;
ProxyShape* mCylinderShape;
// Collision shapes
BoxShape* mBoxShape;
SphereShape* mSphereShape;
CylinderShape* mCylinderShape;
// Proxy shapes
ProxyShape* mBoxProxyShape;
ProxyShape* mSphere1ProxyShape;
ProxyShape* mSphere2ProxyShape;
ProxyShape* mCylinderProxyShape;
// Collision callback class
WorldCollisionCallback mCollisionCallback;
@ -134,28 +141,28 @@ class TestCollisionWorld : public Test {
// Create the bodies
Transform boxTransform(Vector3(10, 0, 0), Quaternion::identity());
mBoxBody = mWorld->createCollisionBody(boxTransform);
BoxShape boxShape(Vector3(3, 3, 3));
mBoxShape = mBoxBody->addCollisionShape(boxShape, Transform::identity());
mBoxShape = new BoxShape(Vector3(3, 3, 3));
mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, Transform::identity());
SphereShape sphereShape(3.0);
mSphereShape = new SphereShape(3.0);
Transform sphere1Transform(Vector3(10,5, 0), Quaternion::identity());
mSphere1Body = mWorld->createCollisionBody(sphere1Transform);
mSphere1Shape = mSphere1Body->addCollisionShape(sphereShape, Transform::identity());
mSphere1ProxyShape = mSphere1Body->addCollisionShape(mSphereShape, Transform::identity());
Transform sphere2Transform(Vector3(30, 10, 10), Quaternion::identity());
mSphere2Body = mWorld->createCollisionBody(sphere2Transform);
mSphere2Shape = mSphere2Body->addCollisionShape(sphereShape, Transform::identity());
mSphere2ProxyShape = mSphere2Body->addCollisionShape(mSphereShape, Transform::identity());
Transform cylinderTransform(Vector3(10, -5, 0), Quaternion::identity());
mCylinderBody = mWorld->createCollisionBody(cylinderTransform);
CylinderShape cylinderShape(2, 5);
mCylinderShape = mCylinderBody->addCollisionShape(cylinderShape, Transform::identity());
mCylinderShape = new CylinderShape(2, 5);
mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, Transform::identity());
// Assign collision categories to proxy shapes
mBoxShape->setCollisionCategoryBits(CATEGORY_1);
mSphere1Shape->setCollisionCategoryBits(CATEGORY_1);
mSphere2Shape->setCollisionCategoryBits(CATEGORY_2);
mCylinderShape->setCollisionCategoryBits(CATEGORY_3);
mBoxProxyShape->setCollisionCategoryBits(CATEGORY_1);
mSphere1ProxyShape->setCollisionCategoryBits(CATEGORY_1);
mSphere2ProxyShape->setCollisionCategoryBits(CATEGORY_2);
mCylinderProxyShape->setCollisionCategoryBits(CATEGORY_3);
mCollisionCallback.boxBody = mBoxBody;
mCollisionCallback.sphere1Body = mSphere1Body;
@ -163,6 +170,13 @@ class TestCollisionWorld : public Test {
mCollisionCallback.cylinderBody = mCylinderBody;
}
/// Destructor
~TestCollisionWorld() {
delete mBoxShape;
delete mSphereShape;
delete mCylinderShape;
}
/// Run the tests
void run() {
@ -183,10 +197,10 @@ class TestCollisionWorld : public Test {
test(!mWorld->testAABBOverlap(mSphere1Body, mCylinderBody));
test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body));
test(mWorld->testAABBOverlap(mBoxShape, mSphere1Shape));
test(mWorld->testAABBOverlap(mBoxShape, mCylinderShape));
test(!mWorld->testAABBOverlap(mSphere1Shape, mCylinderShape));
test(!mWorld->testAABBOverlap(mSphere1Shape, mSphere2Shape));
test(mWorld->testAABBOverlap(mBoxProxyShape, mSphere1ProxyShape));
test(mWorld->testAABBOverlap(mBoxProxyShape, mCylinderProxyShape));
test(!mWorld->testAABBOverlap(mSphere1ProxyShape, mCylinderProxyShape));
test(!mWorld->testAABBOverlap(mSphere1ProxyShape, mSphere2ProxyShape));
mCollisionCallback.reset();
mWorld->testCollision(mCylinderBody, &mCollisionCallback);
@ -210,14 +224,14 @@ class TestCollisionWorld : public Test {
test(!mCollisionCallback.sphere1CollideWithSphere2);
mCollisionCallback.reset();
mWorld->testCollision(mCylinderShape, &mCollisionCallback);
mWorld->testCollision(mCylinderProxyShape, &mCollisionCallback);
test(!mCollisionCallback.boxCollideWithSphere1);
test(mCollisionCallback.boxCollideWithCylinder);
test(!mCollisionCallback.sphere1CollideWithCylinder);
test(!mCollisionCallback.sphere1CollideWithSphere2);
mCollisionCallback.reset();
mWorld->testCollision(mBoxShape, mCylinderShape, &mCollisionCallback);
mWorld->testCollision(mBoxProxyShape, mCylinderProxyShape, &mCollisionCallback);
test(!mCollisionCallback.boxCollideWithSphere1);
test(mCollisionCallback.boxCollideWithCylinder);
test(!mCollisionCallback.sphere1CollideWithCylinder);
@ -268,10 +282,10 @@ class TestCollisionWorld : public Test {
test(!mWorld->testAABBOverlap(mSphere1Body, mCylinderBody));
test(!mWorld->testAABBOverlap(mSphere1Body, mSphere2Body));
test(!mWorld->testAABBOverlap(mBoxShape, mSphere1Shape));
test(!mWorld->testAABBOverlap(mBoxShape, mCylinderShape));
test(!mWorld->testAABBOverlap(mSphere1Shape, mCylinderShape));
test(!mWorld->testAABBOverlap(mSphere1Shape, mSphere2Shape));
test(!mWorld->testAABBOverlap(mBoxProxyShape, mSphere1ProxyShape));
test(!mWorld->testAABBOverlap(mBoxProxyShape, mCylinderProxyShape));
test(!mWorld->testAABBOverlap(mSphere1ProxyShape, mCylinderProxyShape));
test(!mWorld->testAABBOverlap(mSphere1ProxyShape, mSphere2ProxyShape));
mBoxBody->setIsActive(true);
mCylinderBody->setIsActive(true);
@ -280,10 +294,10 @@ class TestCollisionWorld : public Test {
// --------- Test collision with collision filtering -------- //
mBoxShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_3);
mSphere1Shape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_2);
mSphere2Shape->setCollideWithMaskBits(CATEGORY_1);
mCylinderShape->setCollideWithMaskBits(CATEGORY_1);
mBoxProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_3);
mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_1 | CATEGORY_2);
mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_1);
mCylinderProxyShape->setCollideWithMaskBits(CATEGORY_1);
mCollisionCallback.reset();
mWorld->testCollision(&mCollisionCallback);
@ -302,10 +316,10 @@ class TestCollisionWorld : public Test {
test(!mCollisionCallback.sphere1CollideWithCylinder);
test(mCollisionCallback.sphere1CollideWithSphere2);
mBoxShape->setCollideWithMaskBits(CATEGORY_2);
mSphere1Shape->setCollideWithMaskBits(CATEGORY_2);
mSphere2Shape->setCollideWithMaskBits(CATEGORY_3);
mCylinderShape->setCollideWithMaskBits(CATEGORY_1);
mBoxProxyShape->setCollideWithMaskBits(CATEGORY_2);
mSphere1ProxyShape->setCollideWithMaskBits(CATEGORY_2);
mSphere2ProxyShape->setCollideWithMaskBits(CATEGORY_3);
mCylinderProxyShape->setCollideWithMaskBits(CATEGORY_1);
mCollisionCallback.reset();
mWorld->testCollision(&mCollisionCallback);
@ -317,10 +331,10 @@ class TestCollisionWorld : public Test {
// Move sphere 1 to collide with box
mSphere1Body->setTransform(Transform(Vector3(10, 5, 0), Quaternion::identity()));
mBoxShape->setCollideWithMaskBits(0xFFFF);
mSphere1Shape->setCollideWithMaskBits(0xFFFF);
mSphere2Shape->setCollideWithMaskBits(0xFFFF);
mCylinderShape->setCollideWithMaskBits(0xFFFF);
mBoxProxyShape->setCollideWithMaskBits(0xFFFF);
mSphere1ProxyShape->setCollideWithMaskBits(0xFFFF);
mSphere2ProxyShape->setCollideWithMaskBits(0xFFFF);
mCylinderProxyShape->setCollideWithMaskBits(0xFFFF);
}
};

66
test/tests/collision/TestDynamicAABBTree.h Normal file
View File

@ -0,0 +1,66 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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 TEST_DYNAMIC_AABB_TREE_H
#define TEST_DYNAMIC_AABB_TREE_H
// Libraries
#include "Test.h"
#include "collision/broadphase/DynamicAABBTree.h"
/// Reactphysics3D namespace
namespace reactphysics3d {
// Class TestDynamicAABBTree
/**
* Unit test for the dynamic AABB tree
*/
class TestDynamicAABBTree : public Test {
private :
// ---------- Atributes ---------- //
// Dynamic AABB Tree
DynamicAABBTree mTree;
public :
// ---------- Methods ---------- //
/// Constructor
TestDynamicAABBTree(const std::string& name): Test(name) {}
/// Run the tests
void run() {
// TODO : Implement tests here
}
};
}
#endif

564
test/tests/collision/TestPointInside.h
View File

@ -61,20 +61,29 @@ class TestPointInside : public Test {
CollisionBody* mCylinderBody;
CollisionBody* mCompoundBody;
// Collision shapes
BoxShape* mBoxShape;
SphereShape* mSphereShape;
CapsuleShape* mCapsuleShape;
ConeShape* mConeShape;
ConvexMeshShape* mConvexMeshShape;
ConvexMeshShape* mConvexMeshShapeBodyEdgesInfo;
CylinderShape* mCylinderShape;
// Transform
Transform mBodyTransform;
Transform mShapeTransform;
Transform mLocalShapeToWorld;
Transform mLocalShape2ToWorld;
// Collision Shapes
ProxyShape* mBoxShape;
ProxyShape* mSphereShape;
ProxyShape* mCapsuleShape;
ProxyShape* mConeShape;
ProxyShape* mConvexMeshShape;
ProxyShape* mConvexMeshShapeEdgesInfo;
ProxyShape* mCylinderShape;
// Proxy Shapes
ProxyShape* mBoxProxyShape;
ProxyShape* mSphereProxyShape;
ProxyShape* mCapsuleProxyShape;
ProxyShape* mConeProxyShape;
ProxyShape* mConvexMeshProxyShape;
ProxyShape* mConvexMeshProxyShapeEdgesInfo;
ProxyShape* mCylinderProxyShape;
public :
@ -110,65 +119,76 @@ class TestPointInside : public Test {
mLocalShapeToWorld = mBodyTransform * mShapeTransform;
// Create collision shapes
BoxShape boxShape(Vector3(2, 3, 4), 0);
mBoxShape = mBoxBody->addCollisionShape(boxShape, mShapeTransform);
mBoxShape = new BoxShape(Vector3(2, 3, 4), 0);
mBoxProxyShape = mBoxBody->addCollisionShape(mBoxShape, mShapeTransform);
SphereShape sphereShape(3);
mSphereShape = mSphereBody->addCollisionShape(sphereShape, mShapeTransform);
mSphereShape = new SphereShape(3);
mSphereProxyShape = mSphereBody->addCollisionShape(mSphereShape, mShapeTransform);
CapsuleShape capsuleShape(2, 10);
mCapsuleShape = mCapsuleBody->addCollisionShape(capsuleShape, mShapeTransform);
mCapsuleShape = new CapsuleShape(2, 10);
mCapsuleProxyShape = mCapsuleBody->addCollisionShape(mCapsuleShape, mShapeTransform);
ConeShape coneShape(2, 6, 0);
mConeShape = mConeBody->addCollisionShape(coneShape, mShapeTransform);
mConeShape = new ConeShape(2, 6, 0);
mConeProxyShape = mConeBody->addCollisionShape(mConeShape, mShapeTransform);
ConvexMeshShape convexMeshShape(0); // Box of dimension (2, 3, 4)
convexMeshShape.addVertex(Vector3(-2, -3, -4));
convexMeshShape.addVertex(Vector3(2, -3, -4));
convexMeshShape.addVertex(Vector3(2, -3, 4));
convexMeshShape.addVertex(Vector3(-2, -3, 4));
convexMeshShape.addVertex(Vector3(-2, 3, -4));
convexMeshShape.addVertex(Vector3(2, 3, -4));
convexMeshShape.addVertex(Vector3(2, 3, 4));
convexMeshShape.addVertex(Vector3(-2, 3, 4));
mConvexMeshShape = mConvexMeshBody->addCollisionShape(convexMeshShape, mShapeTransform);
mConvexMeshShape = new ConvexMeshShape(0.0); // Box of dimension (2, 3, 4)
mConvexMeshShape->addVertex(Vector3(-2, -3, -4));
mConvexMeshShape->addVertex(Vector3(2, -3, -4));
mConvexMeshShape->addVertex(Vector3(2, -3, 4));
mConvexMeshShape->addVertex(Vector3(-2, -3, 4));
mConvexMeshShape->addVertex(Vector3(-2, 3, -4));
mConvexMeshShape->addVertex(Vector3(2, 3, -4));
mConvexMeshShape->addVertex(Vector3(2, 3, 4));
mConvexMeshShape->addVertex(Vector3(-2, 3, 4));
mConvexMeshProxyShape = mConvexMeshBody->addCollisionShape(mConvexMeshShape, mShapeTransform);
ConvexMeshShape convexMeshShapeEdgesInfo(0);
convexMeshShapeEdgesInfo.addVertex(Vector3(-2, -3, -4));
convexMeshShapeEdgesInfo.addVertex(Vector3(2, -3, -4));
convexMeshShapeEdgesInfo.addVertex(Vector3(2, -3, 4));
convexMeshShapeEdgesInfo.addVertex(Vector3(-2, -3, 4));
convexMeshShapeEdgesInfo.addVertex(Vector3(-2, 3, -4));
convexMeshShapeEdgesInfo.addVertex(Vector3(2, 3, -4));
convexMeshShapeEdgesInfo.addVertex(Vector3(2, 3, 4));
convexMeshShapeEdgesInfo.addVertex(Vector3(-2, 3, 4));
convexMeshShapeEdgesInfo.addEdge(0, 1);
convexMeshShapeEdgesInfo.addEdge(1, 2);
convexMeshShapeEdgesInfo.addEdge(2, 3);
convexMeshShapeEdgesInfo.addEdge(0, 3);
convexMeshShapeEdgesInfo.addEdge(4, 5);
convexMeshShapeEdgesInfo.addEdge(5, 6);
convexMeshShapeEdgesInfo.addEdge(6, 7);
convexMeshShapeEdgesInfo.addEdge(4, 7);
convexMeshShapeEdgesInfo.addEdge(0, 4);
convexMeshShapeEdgesInfo.addEdge(1, 5);
convexMeshShapeEdgesInfo.addEdge(2, 6);
convexMeshShapeEdgesInfo.addEdge(3, 7);
convexMeshShapeEdgesInfo.setIsEdgesInformationUsed(true);
mConvexMeshShapeEdgesInfo = mConvexMeshBodyEdgesInfo->addCollisionShape(
convexMeshShapeEdgesInfo,
mConvexMeshShapeBodyEdgesInfo = new ConvexMeshShape(0.0);
mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, -3, -4));
mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, -3, -4));
mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, -3, 4));
mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, -3, 4));
mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, 3, -4));
mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, 3, -4));
mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(2, 3, 4));
mConvexMeshShapeBodyEdgesInfo->addVertex(Vector3(-2, 3, 4));
mConvexMeshShapeBodyEdgesInfo->addEdge(0, 1);
mConvexMeshShapeBodyEdgesInfo->addEdge(1, 2);
mConvexMeshShapeBodyEdgesInfo->addEdge(2, 3);
mConvexMeshShapeBodyEdgesInfo->addEdge(0, 3);
mConvexMeshShapeBodyEdgesInfo->addEdge(4, 5);
mConvexMeshShapeBodyEdgesInfo->addEdge(5, 6);
mConvexMeshShapeBodyEdgesInfo->addEdge(6, 7);
mConvexMeshShapeBodyEdgesInfo->addEdge(4, 7);
mConvexMeshShapeBodyEdgesInfo->addEdge(0, 4);
mConvexMeshShapeBodyEdgesInfo->addEdge(1, 5);
mConvexMeshShapeBodyEdgesInfo->addEdge(2, 6);
mConvexMeshShapeBodyEdgesInfo->addEdge(3, 7);
mConvexMeshShapeBodyEdgesInfo->setIsEdgesInformationUsed(true);
mConvexMeshProxyShapeEdgesInfo = mConvexMeshBodyEdgesInfo->addCollisionShape(
mConvexMeshShapeBodyEdgesInfo,
mShapeTransform);
CylinderShape cylinderShape(3, 8, 0);
mCylinderShape = mCylinderBody->addCollisionShape(cylinderShape, mShapeTransform);
mCylinderShape = new CylinderShape(3, 8, 0);
mCylinderProxyShape = mCylinderBody->addCollisionShape(mCylinderShape, mShapeTransform);
// Compound shape is a cylinder and a sphere
Vector3 positionShape2(Vector3(4, 2, -3));
Quaternion orientationShape2(-3 *PI / 8, 1.5 * PI/ 3, PI / 13);
Transform shapeTransform2(positionShape2, orientationShape2);
mLocalShape2ToWorld = mBodyTransform * shapeTransform2;
mCompoundBody->addCollisionShape(cylinderShape, mShapeTransform);
mCompoundBody->addCollisionShape(sphereShape, shapeTransform2);
mCompoundBody->addCollisionShape(mCylinderShape, mShapeTransform);
mCompoundBody->addCollisionShape(mSphereShape, shapeTransform2);
}
/// Destructor
~TestPointInside() {
delete mBoxShape;
delete mSphereShape;
delete mCapsuleShape;
delete mConeShape;
delete mConvexMeshShape;
delete mConvexMeshShapeBodyEdgesInfo;
delete mCylinderShape;
}
/// Run the tests
@ -213,30 +233,30 @@ class TestPointInside : public Test {
test(!mBoxBody->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5)));
// Tests with ProxyBoxShape
test(mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
test(mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0)));
test(mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0)));
test(mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0)));
test(mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0)));
test(mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9)));
test(mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9)));
test(mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9)));
test(mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9)));
test(mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5)));
test(mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5)));
test(mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5)));
test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0)));
test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0)));
test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0)));
test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0)));
test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9)));
test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9)));
test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9)));
test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9)));
test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5)));
test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5)));
test(mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5)));
test(!mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0)));
test(!mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0)));
test(!mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0)));
test(!mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0)));
test(!mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1)));
test(!mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1)));
test(!mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1)));
test(!mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1)));
test(!mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5)));
test(!mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5)));
test(!mBoxShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5)));
test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0)));
test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0)));
test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0)));
test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0)));
test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1)));
test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1)));
test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1)));
test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1)));
test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5)));
test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5)));
test(!mBoxProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5)));
}
/// Test the ProxySphereShape::testPointInside() and
@ -266,26 +286,26 @@ class TestPointInside : public Test {
test(!mSphereBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2, 2.5)));
// Tests with ProxySphereShape
test(mSphereShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
test(mSphereShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0)));
test(mSphereShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0)));
test(mSphereShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0)));
test(mSphereShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0)));
test(mSphereShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9)));
test(mSphereShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9)));
test(mSphereShape->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5)));
test(mSphereShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -1.5)));
test(mSphereShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 1.5)));
test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0)));
test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0)));
test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0)));
test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0)));
test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9)));
test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9)));
test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5)));
test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -1.5)));
test(mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 1.5)));
test(!mSphereShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0)));
test(!mSphereShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0)));
test(!mSphereShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0)));
test(!mSphereShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0)));
test(!mSphereShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1)));
test(!mSphereShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1)));
test(!mSphereShape->testPointInside(mLocalShapeToWorld * Vector3(-2, -2, -2)));
test(!mSphereShape->testPointInside(mLocalShapeToWorld * Vector3(-2, 2, -1.5)));
test(!mSphereShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2, 2.5)));
test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0)));
test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0)));
test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0)));
test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0)));
test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1)));
test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1)));
test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2, -2, -2)));
test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2, 2, -1.5)));
test(!mSphereProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2, 2.5)));
}
/// Test the ProxyCapsuleShape::testPointInside() and
@ -340,51 +360,51 @@ class TestPointInside : public Test {
test(!mCapsuleBody->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, -1.7)));
// Tests with ProxyCapsuleShape
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 0)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 0)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0, -6.9, 0)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0, 6.9, 0)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.9)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.9)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0.9)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, -0.9)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 1.9)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -1.9)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 5, 0)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 5, 0)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 5, 0.9)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 5, -0.9)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 1.9)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -1.9)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, -5, 0)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -5, 0)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, -5, 0.9)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, -5, -0.9)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -4, -0.9)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, 0.4)));
test(mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(1.3, 1, 1.5)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 0)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 0)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -6.9, 0)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 6.9, 0)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.9)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.9)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0.9)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, -0.9)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 1.9)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -1.9)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 5, 0)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 5, 0)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 5, 0.9)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 5, -0.9)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 1.9)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -1.9)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, -5, 0)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -5, 0)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, -5, 0.9)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, -5, -0.9)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -4, -0.9)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, 0.4)));
test(mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.3, 1, 1.5)));
test(!mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0, -7.1, 0)));
test(!mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0, 7.1, 0)));
test(!mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.1)));
test(!mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.1)));
test(!mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0)));
test(!mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0)));
test(!mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 2.1)));
test(!mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -2.1)));
test(!mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 5, 0)));
test(!mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 5, 0)));
test(!mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, 1.6)));
test(!mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, -1.7)));
test(!mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 2.1)));
test(!mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -2.1)));
test(!mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, -5, 0)));
test(!mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -5, 0)));
test(!mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, 1.6)));
test(!mCapsuleShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, -1.7)));
test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -7.1, 0)));
test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 7.1, 0)));
test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.1)));
test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.1)));
test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0)));
test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0)));
test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, 2.1)));
test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 5, -2.1)));
test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 5, 0)));
test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 5, 0)));
test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, 1.6)));
test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, 5, -1.7)));
test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, 2.1)));
test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -5, -2.1)));
test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, -5, 0)));
test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -5, 0)));
test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, 1.6)));
test(!mCapsuleProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -5, -1.7)));
}
/// Test the ProxyConeShape::testPointInside() and
@ -428,40 +448,40 @@ class TestPointInside : public Test {
test(!mConeBody->testPointInside(mLocalShapeToWorld * Vector3(-1.5, -2.9, 1.5)));
// Tests with ProxyConeShape
test(mConeShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
test(mConeShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0)));
test(mConeShape->testPointInside(mLocalShapeToWorld * Vector3(-0.9, 0, 0)));
test(mConeShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0.9)));
test(mConeShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -0.9)));
test(mConeShape->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, -0.7)));
test(mConeShape->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, 0.7)));
test(mConeShape->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, -0.7)));
test(mConeShape->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, 0.7)));
test(mConeShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0)));
test(mConeShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0)));
test(mConeShape->testPointInside(mLocalShapeToWorld * Vector3(1.96, -2.9, 0)));
test(mConeShape->testPointInside(mLocalShapeToWorld * Vector3(-1.96, -2.9, 0)));
test(mConeShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.96)));
test(mConeShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.96)));
test(mConeShape->testPointInside(mLocalShapeToWorld * Vector3(1.3, -2.9, -1.4)));
test(mConeShape->testPointInside(mLocalShapeToWorld * Vector3(-1.3, -2.9, 1.4)));
test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.9, 0, 0)));
test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.9, 0, 0)));
test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0.9)));
test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -0.9)));
test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, -0.7)));
test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.6, 0, 0.7)));
test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, -0.7)));
test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.6, 0, 0.7)));
test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0)));
test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0)));
test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.96, -2.9, 0)));
test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.96, -2.9, 0)));
test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.96)));
test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.96)));
test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.3, -2.9, -1.4)));
test(mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.3, -2.9, 1.4)));
test(!mConeShape->testPointInside(mLocalShapeToWorld * Vector3(1.1, 0, 0)));
test(!mConeShape->testPointInside(mLocalShapeToWorld * Vector3(-1.1, 0, 0)));
test(!mConeShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.1)));
test(!mConeShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.1)));
test(!mConeShape->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, -0.8)));
test(!mConeShape->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, 0.8)));
test(!mConeShape->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, -0.8)));
test(!mConeShape->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, 0.8)));
test(!mConeShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0)));
test(!mConeShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0)));
test(!mConeShape->testPointInside(mLocalShapeToWorld * Vector3(1.97, -2.9, 0)));
test(!mConeShape->testPointInside(mLocalShapeToWorld * Vector3(-1.97, -2.9, 0)));
test(!mConeShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.97)));
test(!mConeShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.97)));
test(!mConeShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2.9, -1.5)));
test(!mConeShape->testPointInside(mLocalShapeToWorld * Vector3(-1.5, -2.9, 1.5)));
test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.1, 0, 0)));
test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.1, 0, 0)));
test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 1.1)));
test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -1.1)));
test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, -0.8)));
test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0.8, 0, 0.8)));
test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, -0.8)));
test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-0.8, 0, 0.8)));
test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0)));
test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0)));
test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.97, -2.9, 0)));
test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.97, -2.9, 0)));
test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 1.97)));
test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, -1.97)));
test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.5, -2.9, -1.5)));
test(!mConeProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.5, -2.9, 1.5)));
}
/// Test the ProxyConvexMeshShape::testPointInside() and
@ -497,30 +517,30 @@ class TestPointInside : public Test {
test(!mConvexMeshBody->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5)));
// Tests with ProxyConvexMeshShape
test(mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
test(mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0)));
test(mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0)));
test(mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0)));
test(mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0)));
test(mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9)));
test(mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9)));
test(mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9)));
test(mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9)));
test(mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5)));
test(mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5)));
test(mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5)));
test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0)));
test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0)));
test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0)));
test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0)));
test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9)));
test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9)));
test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9)));
test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9)));
test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5)));
test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5)));
test(mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5)));
test(!mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0)));
test(!mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0)));
test(!mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0)));
test(!mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0)));
test(!mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1)));
test(!mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1)));
test(!mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1)));
test(!mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1)));
test(!mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5)));
test(!mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5)));
test(!mConvexMeshShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5)));
test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0)));
test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0)));
test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0)));
test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0)));
test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1)));
test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1)));
test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1)));
test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1)));
test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5)));
test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5)));
test(!mConvexMeshProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5)));
// ----- Tests using edges information ----- //
@ -551,30 +571,30 @@ class TestPointInside : public Test {
test(!mConvexMeshBodyEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5)));
// Tests with ProxyConvexMeshShape
test(mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
test(mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0)));
test(mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0)));
test(mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0)));
test(mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0)));
test(mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9)));
test(mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9)));
test(mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9)));
test(mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9)));
test(mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5)));
test(mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5)));
test(mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5)));
test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, 0, 0)));
test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 0, 0)));
test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -2.9, 0)));
test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 2.9, 0)));
test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.9)));
test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.9)));
test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1.9, -2.9, -3.9)));
test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1.9, 2.9, 3.9)));
test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, -2, -1.5)));
test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 2, -2.5)));
test(mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 3.5)));
test(!mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0)));
test(!mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0)));
test(!mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0)));
test(!mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0)));
test(!mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1)));
test(!mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1)));
test(!mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1)));
test(!mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1)));
test(!mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5)));
test(!mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5)));
test(!mConvexMeshShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5)));
test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, 0, 0)));
test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 0, 0)));
test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, -3.1, 0)));
test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 3.1, 0)));
test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -4.1)));
test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 4.1)));
test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-2.1, -3.1, -4.1)));
test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(2.1, 3.1, 4.1)));
test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-10, -2, -1.5)));
test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(-1, 4, -2.5)));
test(!mConvexMeshProxyShapeEdgesInfo->testPointInside(mLocalShapeToWorld * Vector3(1, -2, 4.5)));
}
/// Test the ProxyCylinderShape::testPointInside() and
@ -638,60 +658,60 @@ class TestPointInside : public Test {
test(!mCylinderBody->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, 2.2)));
// Tests with ProxyCylinderShape
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.9)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, 1.7)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, -1.7)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, -1.7)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, 1.7)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, 3.9, 0)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 3.9, 0)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 2.9)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -2.9)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, 1.7)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, -1.7)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, -1.7)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, 1.7)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, -3.9, 0)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, -3.9, 0)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 2.9)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -2.9)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, 1.7)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, -1.7)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, -1.7)));
test(mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, 1.7)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 0)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 0)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 0)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, 0, 0)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 0, 0)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 2.9)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -2.9)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, 1.7)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 0, -1.7)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, -1.7)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 0, 1.7)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, 3.9, 0)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, 3.9, 0)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 2.9)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -2.9)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, 1.7)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, 3.9, -1.7)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, -1.7)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, 3.9, 1.7)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.9, -3.9, 0)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.9, -3.9, 0)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 2.9)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -2.9)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, 1.7)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(1.7, -3.9, -1.7)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, -1.7)));
test(mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.7, -3.9, 1.7)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(0, 4.1, 0)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(0, -4.1, 0)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, 2.2)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, -2.2)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 0, -2.2)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(-1.3, 0, 2.8)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 3.9, 0)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 3.9, 0)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 3.1)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -3.1)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, 2.2)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, -2.2)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, -2.2)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, 2.2)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, -3.9, 0)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, -3.9, 0)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 3.1)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -3.1)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, 2.2)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, -2.2)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, -2.2)));
test(!mCylinderShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, 2.2)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 4.1, 0)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -4.1, 0)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 0, 0)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 0, 0)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, 3.1)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 0, -3.1)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, 2.2)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 0, -2.2)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 0, -2.2)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-1.3, 0, 2.8)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, 3.9, 0)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, 3.9, 0)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, 3.1)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, 3.9, -3.1)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, 2.2)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, 3.9, -2.2)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, -2.2)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, 3.9, 2.2)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(3.1, -3.9, 0)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-3.1, -3.9, 0)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, 3.1)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(0, -3.9, -3.1)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, 2.2)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(2.2, -3.9, -2.2)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, -2.2)));
test(!mCylinderProxyShape->testPointInside(mLocalShapeToWorld * Vector3(-2.2, -3.9, 2.2)));
}
/// Test the CollisionBody::testPointInside() method

788
test/tests/collision/TestRaycast.h
View File

File diff suppressed because it is too large Load Diff

129
test/tests/mathematics/TestMathematicsFunctions.h Normal file
View File

@ -0,0 +1,129 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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 TEST_MATHEMATICS_FUNCTIONS_H
#define TEST_MATHEMATICS_FUNCTIONS_H
// Libraries
#include "Test.h"
#include "mathematics/mathematics_functions.h"
/// Reactphysics3D namespace
namespace reactphysics3d {
// Class TestMathematicsFunctions
/**
* Unit test for mathematics functions
*/
class TestMathematicsFunctions : public Test {
private :
// ---------- Atributes ---------- //
public :
// ---------- Methods ---------- //
/// Constructor
TestMathematicsFunctions(const std::string& name): Test(name) {}
/// Run the tests
void run() {
// Test approxEqual()
test(approxEqual(2, 7, 5.2));
test(approxEqual(7, 2, 5.2));
test(approxEqual(6, 6));
test(!approxEqual(1, 5));
test(!approxEqual(1, 5, 3));
test(approxEqual(-2, -2));
test(approxEqual(-2, -7, 6));
test(!approxEqual(-2, 7, 2));
test(approxEqual(-3, 8, 12));
test(!approxEqual(-3, 8, 6));
// Test clamp()
test(clamp(4, -3, 5) == 4);
test(clamp(-3, 1, 8) == 1);
test(clamp(45, -6, 7) == 7);
test(clamp(-5, -2, -1) == -2);
test(clamp(-5, -9, -1) == -5);
test(clamp(6, 6, 9) == 6);
test(clamp(9, 6, 9) == 9);
test(clamp(decimal(4), decimal(-3), decimal(5)) == decimal(4));
test(clamp(decimal(-3), decimal(1), decimal(8)) == decimal(1));
test(clamp(decimal(45), decimal(-6), decimal(7)) == decimal(7));
test(clamp(decimal(-5), decimal(-2), decimal(-1)) == decimal(-2));
test(clamp(decimal(-5), decimal(-9), decimal(-1)) == decimal(-5));
test(clamp(decimal(6), decimal(6), decimal(9)) == decimal(6));
test(clamp(decimal(9), decimal(6), decimal(9)) == decimal(9));
// Test min3()
test(min3(1, 5, 7) == 1);
test(min3(-4, 2, 4) == -4);
test(min3(-1, -5, -7) == -7);
test(min3(13, 5, 47) == 5);
test(min3(4, 4, 4) == 4);
// Test max3()
test(max3(1, 5, 7) == 7);
test(max3(-4, 2, 4) == 4);
test(max3(-1, -5, -7) == -1);
test(max3(13, 5, 47) == 47);
test(max3(4, 4, 4) == 4);
// Test sameSign()
test(sameSign(4, 53));
test(sameSign(-4, -8));
test(!sameSign(4, -7));
test(!sameSign(-4, 53));
// Test computeBarycentricCoordinatesInTriangle()
Vector3 a(0, 0, 0);
Vector3 b(5, 0, 0);
Vector3 c(0, 0, 5);
decimal u,v,w;
computeBarycentricCoordinatesInTriangle(a, b, c, a, u, v, w);
test(approxEqual(u, 1.0, 0.000001));
test(approxEqual(v, 0.0, 0.000001));
test(approxEqual(w, 0.0, 0.000001));
computeBarycentricCoordinatesInTriangle(a, b, c, b, u, v, w);
test(approxEqual(u, 0.0, 0.000001));
test(approxEqual(v, 1.0, 0.000001));
test(approxEqual(w, 0.0, 0.000001));
computeBarycentricCoordinatesInTriangle(a, b, c, c, u, v, w);
test(approxEqual(u, 0.0, 0.000001));
test(approxEqual(v, 0.0, 0.000001));
test(approxEqual(w, 1.0, 0.000001));
}
};
}
#endif

11
test/tests/mathematics/TestVector2.h
View File

@ -201,10 +201,17 @@ class TestVector2 : public Test {
vector4 /= 3;
test(vector3 == Vector2(60, 330));
test(vector4 == Vector2(5, 20));
Vector2 vector5(21, 80);
Vector2 vector6(7, 10);
Vector2 vector7 = vector5 * vector6;
test(vector7 == Vector2(147, 800));
Vector2 vector8 = vector5 / vector6;
test(approxEqual(vector8.x, 3));
test(approxEqual(vector8.y, 8));
// Negative operator
Vector2 vector5(-34, 5);
Vector2 negative = -vector5;
Vector2 vector9(-34, 5);
Vector2 negative = -vector9;
test(negative == Vector2(34, -5));
}
};

12
test/tests/mathematics/TestVector3.h
View File

@ -225,10 +225,18 @@ class TestVector3 : public Test {
vector4 /= 3;
test(vector3 == Vector3(60, 330, 620));
test(vector4 == Vector3(5, 20, 11));
Vector3 vector5(21, 80, 45);
Vector3 vector6(7, 10, 3);
Vector3 vector7 = vector5 * vector6;
test(vector7 == Vector3(147, 800, 135));
Vector3 vector8 = vector5 / vector6;
test(approxEqual(vector8.x, 3));
test(approxEqual(vector8.y, 8));
test(approxEqual(vector8.z, 15));
// Negative operator
Vector3 vector5(-34, 5, 422);
Vector3 negative = -vector5;
Vector3 vector9(-34, 5, 422);
Vector3 negative = -vector9;
test(negative == Vector3(34, -5, -422));
}
};

25
testbed/CMakeLists.txt
View File

@ -22,11 +22,8 @@ FILE(COPY "meshes/" DESTINATION "${EXECUTABLE_OUTPUT_PATH}/meshes/")
# Copy the fonts used for the GUI into the build directory
FILE(COPY "imgui/DroidSans.ttf" DESTINATION "${EXECUTABLE_OUTPUT_PATH}")
# Enable C++11 features
SET(CMAKE_CXX_FLAGS "-Wall -std=c++0x")
# Headers
INCLUDE_DIRECTORIES(${GLEW_INCLUDE_PATH} "src/" "opengl-framework/src/" "glfw/include/" "common/" "scenes/" "imgui/")
INCLUDE_DIRECTORIES("src/" ${GLEW_INCLUDE_PATH} "opengl-framework/src/" "glfw/include/" "common/" "scenes/" "imgui/")
# Testbed source files
SET(TESTBED_SOURCES
@ -66,14 +63,20 @@ SET(COMMON_SOURCES
common/Capsule.cpp
common/ConvexMesh.h
common/ConvexMesh.cpp
common/ConcaveMesh.h
common/ConcaveMesh.cpp
common/Cylinder.h
common/Cylinder.cpp
common/Dumbbell.h
common/Dumbbell.cpp
common/HeightField.h
common/HeightField.cpp
common/PhysicsObject.h
common/PhysicsObject.cpp
common/VisualContactPoint.h
common/VisualContactPoint.cpp
common/PerlinNoise.h
common/PerlinNoise.cpp
)
# Examples scenes source files
@ -86,10 +89,24 @@ SET(SCENES_SOURCES
scenes/raycast/RaycastScene.cpp
scenes/collisionshapes/CollisionShapesScene.h
scenes/collisionshapes/CollisionShapesScene.cpp
scenes/concavemesh/ConcaveMeshScene.h
scenes/concavemesh/ConcaveMeshScene.cpp
scenes/heightfield/HeightFieldScene.h
scenes/heightfield/HeightFieldScene.cpp
)
# Add .user file to set debug path in Visual Studio
SET(USER_FILE testbed.vcxproj.user)
SET(VS_USERFILE_WORKING_DIRECTORY_DEBUG ${EXECUTABLE_OUTPUT_PATH})
SET(OUTPUT_PATH ${CMAKE_CURRENT_BINARY_DIR}/${USER_FILE})
CONFIGURE_FILE(VisualStudioUserTemplate.user ${USER_FILE} @ONLY)
# Create the executable
ADD_EXECUTABLE(testbed ${TESTBED_SOURCES} ${SCENES_SOURCES} ${COMMON_SOURCES} ${IMGUI_SOURCES})
# Enable C++11 features
set_property(TARGET testbed PROPERTY CXX_STANDARD 11)
set_property(TARGET testbed PROPERTY CXX_STANDARD_REQUIRED ON)
# Link with libraries
TARGET_LINK_LIBRARIES(testbed reactphysics3d openglframework glfw ${GLEW_LIBRARIES} ${GLFW_LIBRARIES})

19
testbed/VisualStudioUserTemplate.user Normal file
View File

@ -0,0 +1,19 @@
<?xml version="1.0" encoding="utf-8"?>
<Project ToolsVersion="12.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
<LocalDebuggerWorkingDirectory>@VS_USERFILE_WORKING_DIRECTORY_DEBUG@</LocalDebuggerWorkingDirectory>
<DebuggerFlavor>WindowsLocalDebugger</DebuggerFlavor>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='RelWithDebInfo|Win32'">
<LocalDebuggerWorkingDirectory>@VS_USERFILE_WORKING_DIRECTORY_DEBUG@</LocalDebuggerWorkingDirectory>
<DebuggerFlavor>WindowsLocalDebugger</DebuggerFlavor>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='MinSizeRel|Win32'">
<LocalDebuggerWorkingDirectory>@VS_USERFILE_WORKING_DIRECTORY_DEBUG@</LocalDebuggerWorkingDirectory>
<DebuggerFlavor>WindowsLocalDebugger</DebuggerFlavor>
</PropertyGroup>
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
<LocalDebuggerWorkingDirectory>@VS_USERFILE_WORKING_DIRECTORY_DEBUG@</LocalDebuggerWorkingDirectory>
<DebuggerFlavor>WindowsLocalDebugger</DebuggerFlavor>
</PropertyGroup>
</Project>

25
testbed/common/Box.cpp
View File

@ -132,7 +132,7 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p
// Create the collision shape for the rigid body (box shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::BoxShape collisionShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2]));
mBoxShape = new rp3d::BoxShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2]));
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
@ -145,7 +145,7 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p
mBody = world->createCollisionBody(transform);
// Add the collision shape to the body
mBody->addCollisionShape(collisionShape, rp3d::Transform::identity());
mProxyShape = mBody->addCollisionShape(mBoxShape, rp3d::Transform::identity());
// If the Vertex Buffer object has not been created yet
if (totalNbBoxes == 0) {
@ -181,7 +181,7 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3& p
// Create the collision shape for the rigid body (box shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::BoxShape collisionShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2]));
mBoxShape = new rp3d::BoxShape(rp3d::Vector3(mSize[0], mSize[1], mSize[2]));
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
@ -194,7 +194,7 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3& p
rp3d::RigidBody* body = world->createRigidBody(transform);
// Add the collision shape to the body
body->addCollisionShape(collisionShape, rp3d::Transform::identity(), mass);
mProxyShape = body->addCollisionShape(mBoxShape, rp3d::Transform::identity(), mass);
mBody = body;
@ -220,7 +220,7 @@ Box::~Box() {
mVBONormals.destroy();
mVAO.destroy();
}
delete mBoxShape;
totalNbBoxes--;
}
@ -280,8 +280,6 @@ void Box::render(openglframework::Shader& shader,
shader.unbind();
}
// Create the Vertex Buffer Objects used to render to box with OpenGL.
/// We create two VBOs (one for vertices and one for indices) to render all the boxes
/// in the simulation.
@ -330,3 +328,16 @@ void Box::resetTransform(const rp3d::Transform& transform) {
updateTransform(1.0f);
}
// Set the scaling of the object
void Box::setScaling(const openglframework::Vector3& scaling) {
// Scale the collision shape
mProxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z));
// Scale the graphics object
mScalingMatrix = openglframework::Matrix4(mSize[0] * scaling.x, 0, 0, 0,
0, mSize[1] * scaling.y, 0, 0,
0, 0, mSize[2] * scaling.z, 0,
0, 0, 0, 1);
}

14
testbed/common/Box.h
View File

@ -41,8 +41,11 @@ class Box : public openglframework::Object3D, public PhysicsObject {
/// Size of each side of the box
float mSize[3];
/// Scaling matrix (applied to a cube to obtain the correct box dimensions)
openglframework::Matrix4 mScalingMatrix;
rp3d::BoxShape* mBoxShape;
rp3d::ProxyShape* mProxyShape;
/// Scaling matrix (applied to a cube to obtain the correct box dimensions)
openglframework::Matrix4 mScalingMatrix;
/// Vertex Buffer Object for the vertices data used to render the box with OpenGL
static openglframework::VertexBufferObject mVBOVertices;
@ -88,8 +91,11 @@ class Box : public openglframework::Object3D, public PhysicsObject {
/// Set the position of the box
void resetTransform(const rp3d::Transform& transform);
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor);
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor);
/// Set the scaling of the object
void setScaling(const openglframework::Vector3& scaling);
};
// Update the transform matrix of the object

25
testbed/common/Capsule.cpp
View File

@ -47,7 +47,7 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4(mRadius, 0, 0, 0,
0, (mHeight + 2.0f * mRadius) / 3.0f, 0,0,
0, (mHeight + 2.0f * mRadius) / 3, 0,0,
0, 0, mRadius, 0,
0, 0, 0, 1.0f);
@ -57,7 +57,7 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos
// Create the collision shape for the rigid body (sphere shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::CapsuleShape collisionShape(mRadius, mHeight);
mCapsuleShape = new rp3d::CapsuleShape(mRadius, mHeight);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
@ -70,7 +70,7 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos
mBody = world->createCollisionBody(transform);
// Add a collision shape to the body and specify the mass of the shape
mBody->addCollisionShape(collisionShape, rp3d::Transform::identity());
mProxyShape = mBody->addCollisionShape(mCapsuleShape, rp3d::Transform::identity());
mTransformMatrix = mTransformMatrix * mScalingMatrix;
@ -106,7 +106,7 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos
// Create the collision shape for the rigid body (sphere shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::CapsuleShape collisionShape(mRadius, mHeight);
mCapsuleShape = new rp3d::CapsuleShape(mRadius, mHeight);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
@ -117,7 +117,7 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos
rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform);
// Add a collision shape to the body and specify the mass of the shape
body->addCollisionShape(collisionShape, rp3d::Transform::identity(), mass);
mProxyShape = body->addCollisionShape(mCapsuleShape, rp3d::Transform::identity(), mass);
mBody = body;
@ -146,7 +146,7 @@ Capsule::~Capsule() {
mVBOTextureCoords.destroy();
mVAO.destroy();
}
delete mCapsuleShape;
totalNbCapsules--;
}
@ -281,3 +281,16 @@ void Capsule::resetTransform(const rp3d::Transform& transform) {
updateTransform(1.0f);
}
// Set the scaling of the object
void Capsule::setScaling(const openglframework::Vector3& scaling) {
// Scale the collision shape
mProxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z));
// Scale the graphics object
mScalingMatrix = openglframework::Matrix4(mRadius * scaling.x, 0, 0, 0,
0, (mHeight * scaling.y + 2.0f * mRadius * scaling.x) / 3, 0,0,
0, 0, mRadius * scaling.x, 0,
0, 0, 0, 1.0f);
}

11
testbed/common/Capsule.h
View File

@ -47,6 +47,10 @@ class Capsule : public openglframework::Mesh, public PhysicsObject {
/// Scaling matrix (applied to a sphere to obtain the correct sphere dimensions)
openglframework::Matrix4 mScalingMatrix;
/// Collision shape
rp3d::CapsuleShape* mCapsuleShape;
rp3d::ProxyShape* mProxyShape;
/// Previous transform (for interpolation)
rp3d::Transform mPreviousTransform;
@ -96,8 +100,11 @@ class Capsule : public openglframework::Mesh, public PhysicsObject {
/// Set the position of the box
void resetTransform(const rp3d::Transform& transform);
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor);
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor);
/// Set the scaling of the object
void setScaling(const openglframework::Vector3& scaling);
};
// Update the transform matrix of the object

309
testbed/common/ConcaveMesh.cpp Normal file
View File

@ -0,0 +1,309 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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 "ConcaveMesh.h"
// Constructor
ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position,
reactphysics3d::CollisionWorld* world,
const std::string& meshFolderPath)
: openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER),
mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER),
mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "concavemesh.obj", *this);
// Calculate the normals of the mesh
calculateNormals();
// Initialize the position where the sphere will be rendered
translateWorld(position);
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4::identity();
// For each subpart of the mesh
for (int i=0; i<getNbParts(); i++) {
// Vertex and Indices array for the triangle mesh (data in shared and not copied)
rp3d::TriangleVertexArray* vertexArray =
new rp3d::TriangleVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3),
getNbFaces(i), &(mIndices[i][0]), sizeof(int),
rp3d::TriangleVertexArray::VERTEX_FLOAT_TYPE,
rp3d::TriangleVertexArray::INDEX_INTEGER_TYPE);
// Add the triangle vertex array of the subpart to the triangle mesh
mPhysicsTriangleMesh.addSubpart(vertexArray);
}
// Create the collision shape for the rigid body (convex mesh shape) and
// do not forget to delete it at the end
mConcaveShape = new rp3d::ConcaveMeshShape(&mPhysicsTriangleMesh);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
rp3d::Transform transform(initPosition, initOrientation);
mPreviousTransform = transform;
// Create a rigid body corresponding to the sphere in the dynamics world
mBody = world->createCollisionBody(transform);
// Add a collision shape to the body and specify the mass of the collision shape
mProxyShape = mBody->addCollisionShape(mConcaveShape, rp3d::Transform::identity());
// Create the VBOs and VAO
createVBOAndVAO();
mTransformMatrix = mTransformMatrix * mScalingMatrix;
}
// Constructor
ConcaveMesh::ConcaveMesh(const openglframework::Vector3 &position, float mass,
reactphysics3d::DynamicsWorld* dynamicsWorld,
const std::string& meshFolderPath)
: openglframework::Mesh(), mVBOVertices(GL_ARRAY_BUFFER),
mVBONormals(GL_ARRAY_BUFFER), mVBOTextureCoords(GL_ARRAY_BUFFER),
mVBOIndices(GL_ELEMENT_ARRAY_BUFFER) {
// Load the mesh from a file
openglframework::MeshReaderWriter::loadMeshFromFile(meshFolderPath + "concavemesh.obj", *this);
// Calculate the normals of the mesh
calculateNormals();
// Initialize the position where the sphere will be rendered
translateWorld(position);
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4::identity();
// For each subpart of the mesh
for (int i=0; i<getNbParts(); i++) {
// Vertex and Indices array for the triangle mesh (data in shared and not copied)
rp3d::TriangleVertexArray* vertexArray =
new rp3d::TriangleVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3),
getNbFaces(i), &(mIndices[i][0]), sizeof(int),
rp3d::TriangleVertexArray::VERTEX_FLOAT_TYPE,
rp3d::TriangleVertexArray::INDEX_INTEGER_TYPE);
// Add the triangle vertex array of the subpart to the triangle mesh
mPhysicsTriangleMesh.addSubpart(vertexArray);
}
// Create the collision shape for the rigid body (convex mesh shape) and
// do not forget to delete it at the end
mConcaveShape = new rp3d::ConcaveMeshShape(&mPhysicsTriangleMesh);
mConcaveShape->setIsSmoothMeshCollisionEnabled(false);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the sphere in the dynamics world
rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform);
// Add a collision shape to the body and specify the mass of the collision shape
mProxyShape = body->addCollisionShape(mConcaveShape, rp3d::Transform::identity(), mass);
mBody = body;
// Create the VBOs and VAO
createVBOAndVAO();
mTransformMatrix = mTransformMatrix * mScalingMatrix;
}
// Destructor
ConcaveMesh::~ConcaveMesh() {
// Destroy the triangle mesh data for the physics engine
for (int i=0; i<mPhysicsTriangleMesh.getNbSubparts(); i++) {
delete mPhysicsTriangleMesh.getSubpart(i);
}
// Destroy the mesh
destroy();
// Destroy the VBOs and VAO
mVBOIndices.destroy();
mVBOVertices.destroy();
mVBONormals.destroy();
mVBOTextureCoords.destroy();
mVAO.destroy();
delete mConcaveShape;
}
// Render the sphere at the correct position and with the correct orientation
void ConcaveMesh::render(openglframework::Shader& shader,
const openglframework::Matrix4& worldToCameraMatrix) {
// Bind the shader
shader.bind();
// Set the model to camera matrix
shader.setMatrix4x4Uniform("localToWorldMatrix", mTransformMatrix);
shader.setMatrix4x4Uniform("worldToCameraMatrix", worldToCameraMatrix);
// Set the normal matrix (inverse transpose of the 3x3 upper-left sub matrix of the
// model-view matrix)
const openglframework::Matrix4 localToCameraMatrix = worldToCameraMatrix * mTransformMatrix;
const openglframework::Matrix3 normalMatrix =
localToCameraMatrix.getUpperLeft3x3Matrix().getInverse().getTranspose();
shader.setMatrix3x3Uniform("normalMatrix", normalMatrix, false);
// Set the vertex color
openglframework::Vector4 color(mColor.r, mColor.g, mColor.b, mColor.a);
shader.setVector4Uniform("vertexColor", color, false);
// Bind the VAO
mVAO.bind();
mVBOVertices.bind();
// Get the location of shader attribute variables
GLint vertexPositionLoc = shader.getAttribLocation("vertexPosition");
GLint vertexNormalLoc = shader.getAttribLocation("vertexNormal", false);
glEnableVertexAttribArray(vertexPositionLoc);
glVertexAttribPointer(vertexPositionLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL);
mVBONormals.bind();
if (vertexNormalLoc != -1) glVertexAttribPointer(vertexNormalLoc, 3, GL_FLOAT, GL_FALSE, 0, (char*)NULL);
if (vertexNormalLoc != -1) glEnableVertexAttribArray(vertexNormalLoc);
// For each part of the mesh
for (unsigned int i=0; i<getNbParts(); i++) {
glDrawElements(GL_TRIANGLES, getNbFaces(i) * 3, GL_UNSIGNED_INT, (char*)NULL);
}
glDisableVertexAttribArray(vertexPositionLoc);
if (vertexNormalLoc != -1) glDisableVertexAttribArray(vertexNormalLoc);
mVBONormals.unbind();
mVBOVertices.unbind();
// Unbind the VAO
mVAO.unbind();
// Unbind the shader
shader.unbind();
}
// Create the Vertex Buffer Objects used to render with OpenGL.
/// We create two VBOs (one for vertices and one for indices)
void ConcaveMesh::createVBOAndVAO() {
// Create the VBO for the vertices data
mVBOVertices.create();
mVBOVertices.bind();
size_t sizeVertices = mVertices.size() * sizeof(openglframework::Vector3);
mVBOVertices.copyDataIntoVBO(sizeVertices, getVerticesPointer(), GL_STATIC_DRAW);
mVBOVertices.unbind();
// Create the VBO for the normals data
mVBONormals.create();
mVBONormals.bind();
size_t sizeNormals = mNormals.size() * sizeof(openglframework::Vector3);
mVBONormals.copyDataIntoVBO(sizeNormals, getNormalsPointer(), GL_STATIC_DRAW);
mVBONormals.unbind();
if (hasTexture()) {
// Create the VBO for the texture co data
mVBOTextureCoords.create();
mVBOTextureCoords.bind();
size_t sizeTextureCoords = mUVs.size() * sizeof(openglframework::Vector2);
mVBOTextureCoords.copyDataIntoVBO(sizeTextureCoords, getUVTextureCoordinatesPointer(), GL_STATIC_DRAW);
mVBOTextureCoords.unbind();
}
// Create th VBO for the indices data
mVBOIndices.create();
mVBOIndices.bind();
size_t sizeIndices = mIndices[0].size() * sizeof(unsigned int);
mVBOIndices.copyDataIntoVBO(sizeIndices, getIndicesPointer(), GL_STATIC_DRAW);
mVBOIndices.unbind();
// Create the VAO for both VBOs
mVAO.create();
mVAO.bind();
// Bind the VBO of vertices
mVBOVertices.bind();
// Bind the VBO of normals
mVBONormals.bind();
if (hasTexture()) {
// Bind the VBO of texture coords
mVBOTextureCoords.bind();
}
// Bind the VBO of indices
mVBOIndices.bind();
// Unbind the VAO
mVAO.unbind();
}
// Reset the transform
void ConcaveMesh::resetTransform(const rp3d::Transform& transform) {
// Reset the transform
mBody->setTransform(transform);
mBody->setIsSleeping(false);
// Reset the velocity of the rigid body
rp3d::RigidBody* rigidBody = dynamic_cast<rp3d::RigidBody*>(mBody);
if (rigidBody != NULL) {
rigidBody->setLinearVelocity(rp3d::Vector3(0, 0, 0));
rigidBody->setAngularVelocity(rp3d::Vector3(0, 0, 0));
}
updateTransform(1.0f);
}
// Set the scaling of the object
void ConcaveMesh::setScaling(const openglframework::Vector3& scaling) {
// Scale the collision shape
mProxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z));
// Scale the graphics object
mScalingMatrix = openglframework::Matrix4(scaling.x, 0, 0, 0,
0, scaling.y, 0,0,
0, 0, scaling.z, 0,
0, 0, 0, 1.0f);
}

108
testbed/common/ConcaveMesh.h Normal file
View File

@ -0,0 +1,108 @@
/********************************************************************************
* ReactPhysics3D physics library, http://www.reactphysics3d.com *
* Copyright (c) 2010-2015 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 CONCAVE_MESH_H
#define CONCAVE_MESH_H
// Libraries
#include "openglframework.h"
#include "reactphysics3d.h"
#include "PhysicsObject.h"
// Class ConcaveMesh
class ConcaveMesh : public openglframework::Mesh, public PhysicsObject {
private :
// -------------------- Attributes -------------------- //
/// Previous transform (for interpolation)
rp3d::Transform mPreviousTransform;
/// Collision shape
rp3d::ConcaveMeshShape* mConcaveShape;
rp3d::ProxyShape* mProxyShape;
/// Scaling matrix
openglframework::Matrix4 mScalingMatrix;
/// Vertex Buffer Object for the vertices data
openglframework::VertexBufferObject mVBOVertices;
/// Vertex Buffer Object for the normals data
openglframework::VertexBufferObject mVBONormals;
/// Vertex Buffer Object for the texture coords
openglframework::VertexBufferObject mVBOTextureCoords;
/// Vertex Buffer Object for the indices
openglframework::VertexBufferObject mVBOIndices;
/// Vertex Array Object for the vertex data
openglframework::VertexArrayObject mVAO;
/// Structure with pointer to the shared mesh data (vertices, indices, ...)
rp3d::TriangleMesh mPhysicsTriangleMesh;
// -------------------- Methods -------------------- //
// Create the Vertex Buffer Objects used to render with OpenGL.
void createVBOAndVAO();
public :
// -------------------- Methods -------------------- //
/// Constructor
ConcaveMesh(const openglframework::Vector3& position,
rp3d::CollisionWorld* world, const std::string& meshFolderPath);
/// Constructor
ConcaveMesh(const openglframework::Vector3& position, float mass,
rp3d::DynamicsWorld* dynamicsWorld, const std::string& meshFolderPath);
/// Destructor
~ConcaveMesh();
/// Render the mesh at the correct position and with the correct orientation
void render(openglframework::Shader& shader,
const openglframework::Matrix4& worldToCameraMatrix);
/// Set the position of the box
void resetTransform(const rp3d::Transform& transform);
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor);
/// Set the scaling of the object
void setScaling(const openglframework::Vector3& scaling);
};
// Update the transform matrix of the object
inline void ConcaveMesh::updateTransform(float interpolationFactor) {
mTransformMatrix = computeTransform(interpolationFactor, mScalingMatrix);
}
#endif

33
testbed/common/Cone.cpp
View File

@ -54,10 +54,9 @@ Cone::Cone(float radius, float height, const openglframework::Vector3 &position,
// Initialize the position where the cone will be rendered
translateWorld(position);
// Create the collision shape for the rigid body (cone shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::ConeShape collisionShape(mRadius, mHeight);
// Create the collision shape for the rigid body (cone shape) and do
// not forget to delete it at the end
mConeShape = new rp3d::ConeShape(mRadius, mHeight);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
@ -70,7 +69,7 @@ Cone::Cone(float radius, float height, const openglframework::Vector3 &position,
mBody = world->createCollisionBody(transform);
// Add a collision shape to the body and specify the mass of the shape
mBody->addCollisionShape(collisionShape, rp3d::Transform::identity());
mProxyShape = mBody->addCollisionShape(mConeShape, rp3d::Transform::identity());
mTransformMatrix = mTransformMatrix * mScalingMatrix;
@ -103,10 +102,9 @@ Cone::Cone(float radius, float height, const openglframework::Vector3 &position,
// Initialize the position where the cone will be rendered
translateWorld(position);
// Create the collision shape for the rigid body (cone shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::ConeShape collisionShape(mRadius, mHeight);
// Create the collision shape for the rigid body (cone shape) and do not
// forget to delete it at the end
mConeShape = new rp3d::ConeShape(mRadius, mHeight);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
@ -117,7 +115,7 @@ Cone::Cone(float radius, float height, const openglframework::Vector3 &position,
rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform);
// Add a collision shape to the body and specify the mass of the shape
body->addCollisionShape(collisionShape, rp3d::Transform::identity(), mass);
mProxyShape = body->addCollisionShape(mConeShape, rp3d::Transform::identity(), mass);
mBody = body;
@ -145,7 +143,7 @@ Cone::~Cone() {
mVBOTextureCoords.destroy();
mVAO.destroy();
}
delete mConeShape;
totalNbCones--;
}
@ -280,3 +278,16 @@ void Cone::resetTransform(const rp3d::Transform& transform) {
updateTransform(1.0f);
}
// Set the scaling of the object
void Cone::setScaling(const openglframework::Vector3& scaling) {
// Scale the collision shape
mProxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z));
// Scale the graphics object
mScalingMatrix = openglframework::Matrix4(mRadius * scaling.x, 0, 0, 0,
0, mHeight * scaling.y, 0, 0,
0, 0, mRadius * scaling.z, 0,
0, 0, 0, 1);
}

7
testbed/common/Cone.h
View File

@ -44,6 +44,10 @@ class Cone : public openglframework::Mesh, public PhysicsObject {
/// Height of the cone
float mHeight;
/// Collision shape
rp3d::ConeShape* mConeShape;
rp3d::ProxyShape* mProxyShape;
/// Scaling matrix (applied to a sphere to obtain the correct cone dimensions)
openglframework::Matrix4 mScalingMatrix;
@ -97,6 +101,9 @@ class Cone : public openglframework::Mesh, public PhysicsObject {
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor);
/// Set the scaling of the object
void setScaling(const openglframework::Vector3& scaling);
};
inline void Cone::updateTransform(float interpolationFactor) {

104
testbed/common/ConvexMesh.cpp
View File

@ -43,38 +43,20 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position,
// Initialize the position where the sphere will be rendered
translateWorld(position);
// Convert the vertices array to the rp3d::decimal type
rp3d::decimal* vertices = new rp3d::decimal[3 * mVertices.size()];
for (int i=0; i < mVertices.size(); i++) {
vertices[3 * i] = static_cast<rp3d::decimal>(mVertices[i].x);
vertices[3 * i + 1] = static_cast<rp3d::decimal>(mVertices[i].y);
vertices[3 * i + 2] = static_cast<rp3d::decimal>(mVertices[i].z);
}
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4::identity();
// Create the collision shape for the rigid body (convex mesh shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
rp3d::ConvexMeshShape collisionShape(vertices, mVertices.size(), 3 * sizeof(rp3d::decimal));
// Vertex and Indices array for the triangle mesh (data in shared and not copied)
mPhysicsTriangleVertexArray =
new rp3d::TriangleVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3),
getNbFaces(0), &(mIndices[0][0]), sizeof(int),
rp3d::TriangleVertexArray::VERTEX_FLOAT_TYPE,
rp3d::TriangleVertexArray::INDEX_INTEGER_TYPE);
delete[] vertices;
// Add the edges information of the mesh into the convex mesh collision shape.
// This is optional but it really speed up the convex mesh collision detection at the
// cost of some additional memory to store the edges inside the collision shape.
for (unsigned int i=0; i<getNbFaces(); i++) { // For each triangle face of the mesh
// Get the three vertex IDs of the vertices of the face
unsigned int v1 = getVertexIndexInFace(i, 0);
unsigned int v2 = getVertexIndexInFace(i, 1);
unsigned int v3 = getVertexIndexInFace(i, 2);
// Add the three edges into the collision shape
collisionShape.addEdge(v1, v2);
collisionShape.addEdge(v1, v3);
collisionShape.addEdge(v2, v3);
}
collisionShape.setIsEdgesInformationUsed(true);// Enable the fast collision detection with edges
// Create the collision shape for the rigid body (convex mesh shape) and
// do not forget to delete it at the end
mConvexShape = new rp3d::ConvexMeshShape(mPhysicsTriangleVertexArray);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
@ -87,10 +69,12 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position,
mBody = world->createCollisionBody(transform);
// Add a collision shape to the body and specify the mass of the collision shape
mBody->addCollisionShape(collisionShape, rp3d::Transform::identity());
mProxyShape = mBody->addCollisionShape(mConvexShape, rp3d::Transform::identity());
// Create the VBOs and VAO
createVBOAndVAO();
mTransformMatrix = mTransformMatrix * mScalingMatrix;
}
// Constructor
@ -110,37 +94,19 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass,
// Initialize the position where the sphere will be rendered
translateWorld(position);
// Convert the vertices array to the rp3d::decimal type
rp3d::decimal* vertices = new rp3d::decimal[3 * mVertices.size()];
for (int i=0; i < mVertices.size(); i++) {
vertices[3 * i] = static_cast<rp3d::decimal>(mVertices[i].x);
vertices[3 * i + 1] = static_cast<rp3d::decimal>(mVertices[i].y);
vertices[3 * i + 2] = static_cast<rp3d::decimal>(mVertices[i].z);
}
// Compute the scaling matrix
mScalingMatrix = openglframework::Matrix4::identity();
// Create the collision shape for the rigid body (convex mesh shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
rp3d::ConvexMeshShape collisionShape(vertices, mVertices.size(), 3 * sizeof(rp3d::decimal));
// Vertex and Indices array for the triangle mesh (data in shared and not copied)
mPhysicsTriangleVertexArray =
new rp3d::TriangleVertexArray(getNbVertices(), &(mVertices[0]), sizeof(openglframework::Vector3),
getNbFaces(0), &(mIndices[0][0]), sizeof(int),
rp3d::TriangleVertexArray::VERTEX_FLOAT_TYPE,
rp3d::TriangleVertexArray::INDEX_INTEGER_TYPE);
delete[] vertices;
// Add the edges information of the mesh into the convex mesh collision shape.
// This is optional but it really speed up the convex mesh collision detection at the
// cost of some additional memory to store the edges inside the collision shape.
for (unsigned int i=0; i<getNbFaces(); i++) { // For each triangle face of the mesh
// Get the three vertex IDs of the vertices of the face
unsigned int v1 = getVertexIndexInFace(i, 0);
unsigned int v2 = getVertexIndexInFace(i, 1);
unsigned int v3 = getVertexIndexInFace(i, 2);
// Add the three edges into the collision shape
collisionShape.addEdge(v1, v2);
collisionShape.addEdge(v1, v3);
collisionShape.addEdge(v2, v3);
}
collisionShape.setIsEdgesInformationUsed(true);// Enable the fast collision detection with edges
// Create the collision shape for the rigid body (convex mesh shape) and do
// not forget to delete it at the end
mConvexShape = new rp3d::ConvexMeshShape(mPhysicsTriangleVertexArray);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
@ -151,12 +117,14 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass,
rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform);
// Add a collision shape to the body and specify the mass of the collision shape
body->addCollisionShape(collisionShape, rp3d::Transform::identity(), mass);
mProxyShape = body->addCollisionShape(mConvexShape, rp3d::Transform::identity(), mass);
mBody = body;
// Create the VBOs and VAO
createVBOAndVAO();
mTransformMatrix = mTransformMatrix * mScalingMatrix;
}
// Destructor
@ -171,6 +139,9 @@ ConvexMesh::~ConvexMesh() {
mVBONormals.destroy();
mVBOTextureCoords.destroy();
mVAO.destroy();
delete mPhysicsTriangleVertexArray;
delete mConvexShape;
}
// Render the sphere at the correct position and with the correct orientation
@ -303,3 +274,16 @@ void ConvexMesh::resetTransform(const rp3d::Transform& transform) {
updateTransform(1.0f);
}
// Set the scaling of the object
void ConvexMesh::setScaling(const openglframework::Vector3& scaling) {
// Scale the collision shape
mProxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z));
// Scale the graphics object
mScalingMatrix = openglframework::Matrix4(scaling.x, 0, 0, 0,
0, scaling.y, 0, 0,
0, 0, scaling.z, 0,
0, 0, 0, 1);
}

14
testbed/common/ConvexMesh.h
View File

@ -41,6 +41,15 @@ class ConvexMesh : public openglframework::Mesh, public PhysicsObject {
/// Previous transform (for interpolation)
rp3d::Transform mPreviousTransform;
rp3d::TriangleVertexArray* mPhysicsTriangleVertexArray;
/// Collision shape
rp3d::ConvexMeshShape* mConvexShape;
rp3d::ProxyShape* mProxyShape;
/// Scaling matrix
openglframework::Matrix4 mScalingMatrix;
/// Vertex Buffer Object for the vertices data
openglframework::VertexBufferObject mVBOVertices;
@ -85,11 +94,14 @@ class ConvexMesh : public openglframework::Mesh, public PhysicsObject {
/// Update the transform matrix of the object
virtual void updateTransform(float interpolationFactor);
/// Set the scaling of the object
void setScaling(const openglframework::Vector3& scaling);
};
// Update the transform matrix of the object
inline void ConvexMesh::updateTransform(float interpolationFactor) {
mTransformMatrix = computeTransform(interpolationFactor, openglframework::Matrix4::identity());
mTransformMatrix = computeTransform(interpolationFactor, mScalingMatrix);
}
#endif

33
testbed/common/Cylinder.cpp
View File

@ -54,10 +54,9 @@ Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& p
// Initialize the position where the cylinder will be rendered
translateWorld(position);
// Create the collision shape for the rigid body (cylinder shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::CylinderShape collisionShape(mRadius, mHeight);
// Create the collision shape for the rigid body (cylinder shape) and do not
// forget to delete it at the end
mCylinderShape = new rp3d::CylinderShape(mRadius, mHeight);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
@ -70,7 +69,7 @@ Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& p
mBody = world->createCollisionBody(transform);
// Add a collision shape to the body and specify the mass of the shape
mBody->addCollisionShape(collisionShape, rp3d::Transform::identity());
mProxyShape = mBody->addCollisionShape(mCylinderShape, rp3d::Transform::identity());
mTransformMatrix = mTransformMatrix * mScalingMatrix;
@ -103,10 +102,9 @@ Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& p
// Initialize the position where the cylinder will be rendered
translateWorld(position);
// Create the collision shape for the rigid body (cylinder shape)
// ReactPhysics3D will clone this object to create an internal one. Therefore,
// it is OK if this object is destroyed right after calling RigidBody::addCollisionShape()
const rp3d::CylinderShape collisionShape(mRadius, mHeight);
// Create the collision shape for the rigid body (cylinder shape) and do
// not forget to delete it at the end
mCylinderShape = new rp3d::CylinderShape(mRadius, mHeight);
// Initial position and orientation of the rigid body
rp3d::Vector3 initPosition(position.x, position.y, position.z);
@ -117,7 +115,7 @@ Cylinder::Cylinder(float radius, float height, const openglframework::Vector3& p
rp3d::RigidBody* body = dynamicsWorld->createRigidBody(transform);
// Add a collision shape to the body and specify the mass of the shape
body->addCollisionShape(collisionShape, rp3d::Transform::identity(), mass);
mProxyShape = body->addCollisionShape(mCylinderShape, rp3d::Transform::identity(), mass);
mTransformMatrix = mTransformMatrix * mScalingMatrix;
@ -146,7 +144,7 @@ Cylinder::~Cylinder() {
mVBOTextureCoords.destroy();
mVAO.destroy();
}
delete mCylinderShape;
totalNbCylinders--;
}
@ -280,3 +278,16 @@ void Cylinder::resetTransform(const rp3d::Transform& transform) {
updateTransform(1.0f);
}
// Set the scaling of the object
void Cylinder::setScaling(const openglframework::Vector3& scaling) {
// Scale the collision shape
mProxyShape->setLocalScaling(rp3d::Vector3(scaling.x, scaling.y, scaling.z));
// Scale the graphics object
mScalingMatrix = openglframework::Matrix4(mRadius * scaling.x, 0, 0, 0,
0, mHeight * scaling.y, 0, 0,
0, 0, mRadius * scaling.z, 0,
0, 0, 0, 1);
}

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