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

continue to replace SAP broad-phase by a dynamic AABB tree

Browse Source
This commit is contained in:
Daniel Chappuis 2014-05-15 06:39:39 +02:00
parent a448334013
commit aa76c85e60
41 changed files with 322 additions and 1370 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
CMakeLists.txt
View File

@ -43,10 +43,6 @@ SET (REACTPHYSICS3D_SOURCES
"src/body/RigidBody.cpp"
"src/collision/broadphase/BroadPhaseAlgorithm.h"
"src/collision/broadphase/BroadPhaseAlgorithm.cpp"
"src/collision/broadphase/NoBroadPhaseAlgorithm.h"
"src/collision/broadphase/NoBroadPhaseAlgorithm.cpp"
"src/collision/broadphase/SweepAndPruneAlgorithm.h"
"src/collision/broadphase/SweepAndPruneAlgorithm.cpp"
"src/collision/broadphase/DynamicAABBTree.h"
"src/collision/broadphase/DynamicAABBTree.cpp"
"src/collision/narrowphase/EPA/EdgeEPA.h"

7
examples/common/Box.cpp
View File

@ -85,8 +85,11 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the cube in the dynamics world
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
// Create a rigid body in the dynamics world
mRigidBody = dynamicsWorld->createRigidBody(transform);
// Add the collision shape to the body
mRigidBody->addCollisionShape(collisionShape, mass);
// If the Vertex Buffer object has not been created yet
if (!areVBOsCreated) {

7
examples/common/Capsule.cpp
View File

@ -58,8 +58,11 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the sphere in the dynamics world
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
// Create a rigid body corresponding in the dynamics world
mRigidBody = dynamicsWorld->createRigidBody(transform);
// Add a collision shape to the body and specify the mass of the shape
mRigidBody->addCollisionShape(collisionShape, mass);
}
// Destructor

5
examples/common/Cone.cpp
View File

@ -59,7 +59,10 @@ Cone::Cone(float radius, float height, const openglframework::Vector3 &position,
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the cone in the dynamics world
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
mRigidBody = dynamicsWorld->createRigidBody(transform);
// Add a collision shape to the body and specify the mass of the shape
mRigidBody->addCollisionShape(collisionShape, mass);
}
// Destructor

5
examples/common/ConvexMesh.cpp
View File

@ -72,7 +72,10 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass,
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the sphere in the dynamics world
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
mRigidBody = dynamicsWorld->createRigidBody(transform);
// Add a collision shape to the body and specify the mass of the collision shape
mRigidBody->addCollisionShape(collisionShape, mass);
}
// Destructor

5
examples/common/Cylinder.cpp
View File

@ -59,7 +59,10 @@ Cylinder::Cylinder(float radius, float height, const openglframework::Vector3 &p
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the cylinder in the dynamics world
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
mRigidBody = dynamicsWorld->createRigidBody(transform);
// Add a collision shape to the body and specify the mass of the shape
mRigidBody->addCollisionShape(collisionShape, mass);
}
// Destructor

5
examples/common/Sphere.cpp
View File

@ -59,7 +59,10 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position,
rp3d::Transform transform(initPosition, initOrientation);
// Create a rigid body corresponding to the sphere in the dynamics world
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
mRigidBody = dynamicsWorld->createRigidBody(transform);
// Add a collision shape to the body and specify the mass of the shape
mRigidBody->addCollisionShape(collisionShape, mass);
}
// Destructor

11
src/body/CollisionBody.cpp
View File

@ -62,7 +62,7 @@ CollisionBody::~CollisionBody() {
/// This method will return a pointer to the proxy collision shape that links the body with
/// the collision shape you have added.
const ProxyShape* CollisionBody::addCollisionShape(const CollisionShape& collisionShape,
const Transform& transform) {
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);
@ -90,14 +90,15 @@ const ProxyShape* CollisionBody::addCollisionShape(const CollisionShape& collisi
}
// Remove a collision shape from the body
void CollisionBody::removeCollisionShape(const CollisionShape* collisionShape) {
void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
ProxyShape* current = mProxyCollisionShapes;
// If the the first proxy shape is the one to remove
if (current->getCollisionShape() == collisionShape) {
if (current == proxyShape) {
mProxyCollisionShapes = current->mNext;
mWorld.mCollisionDetection.removeProxyCollisionShape(current);
mWorld.removeCollisionShape(proxyShape->getInternalCollisionShape());
size_t sizeBytes = current->getSizeInBytes();
current->ProxyShape::~ProxyShape();
mWorld.mMemoryAllocator.release(current, sizeBytes);
@ -109,12 +110,13 @@ void CollisionBody::removeCollisionShape(const CollisionShape* collisionShape) {
while(current->mNext != NULL) {
// If we have found the collision shape to remove
if (current->mNext->getCollisionShape() == collisionShape) {
if (current->mNext == proxyShape) {
// Remove the proxy collision shape
ProxyShape* elementToRemove = current->mNext;
current->mNext = elementToRemove->mNext;
mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove);
mWorld.removeCollisionShape(proxyShape->getInternalCollisionShape());
size_t sizeBytes = elementToRemove->getSizeInBytes();
elementToRemove->ProxyShape::~ProxyShape();
mWorld.mMemoryAllocator.release(elementToRemove, sizeBytes);
@ -140,6 +142,7 @@ void CollisionBody::removeAllCollisionShapes() {
// Remove the proxy collision shape
ProxyShape* nextElement = current->mNext;
mWorld.mCollisionDetection.removeProxyCollisionShape(current);
mWorld.removeCollisionShape(current->getInternalCollisionShape());
current->ProxyShape::~ProxyShape();
mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape));

5
src/body/CollisionBody.h
View File

@ -137,10 +137,10 @@ class CollisionBody : public Body {
/// Add a collision shape to the body.
const ProxyShape* addCollisionShape(const CollisionShape& collisionShape,
const Transform& transform = Transform::identity());
const Transform& transform = Transform::identity());
/// Remove a collision shape from the body
virtual void removeCollisionShape(const CollisionShape* collisionShape);
virtual void removeCollisionShape(const ProxyShape* proxyShape);
/// Return the interpolated transform for rendering
Transform getInterpolatedTransform() const;
@ -159,6 +159,7 @@ class CollisionBody : public Body {
// -------------------- Friendship -------------------- //
friend class CollisionWorld;
friend class DynamicsWorld;
friend class CollisionDetection;
friend class BroadPhaseAlgorithm;

21
src/body/RigidBody.cpp
View File

@ -130,15 +130,14 @@ void RigidBody::removeJointFromJointsList(MemoryAllocator& memoryAllocator, cons
/// return a pointer to the actual collision shape in the world. You can use this pointer to
/// remove the collision from the body. Note that when the body is destroyed, all the collision
/// shapes will also be destroyed automatically. Because an internal copy of the collision shape
/// you provided is performed, you can delete it right after calling this method. The second
/// you provided is performed, you can delete it right after calling this method.
/// The second parameter is the mass of the collision shape (this will used to compute the
/// total mass of the rigid body and its inertia tensor). The mass must be positive. The third
/// parameter is the transformation that transform the local-space of the collision shape into
/// the local-space of the body. By default, the second parameter is the identity transform.
/// The third parameter is the mass of the collision shape (this will used to compute the
/// total mass of the rigid body and its inertia tensor). The mass must be positive.
const CollisionShape* RigidBody::addCollisionShape(const CollisionShape& collisionShape,
decimal mass,
const Transform& transform
) {
const ProxyShape* RigidBody::addCollisionShape(const CollisionShape& collisionShape,
decimal mass,
const Transform& transform) {
assert(mass > decimal(0.0));
@ -167,15 +166,15 @@ const CollisionShape* RigidBody::addCollisionShape(const CollisionShape& collisi
// collision shape
recomputeMassInformation();
// Return a pointer to the collision shape
return newCollisionShape;
// Return a pointer to the proxy collision shape
return proxyShape;
}
// Remove a collision shape from the body
void RigidBody::removeCollisionShape(const CollisionShape* collisionShape) {
void RigidBody::removeCollisionShape(const ProxyShape* proxyCollisionShape) {
// Remove the collision shape
CollisionBody::removeCollisionShape(collisionShape);
CollisionBody::removeCollisionShape(proxyCollisionShape);
// Recompute the total mass, center of mass and inertia tensor
recomputeMassInformation();

8
src/body/RigidBody.h
View File

@ -195,12 +195,12 @@ class RigidBody : public CollisionBody {
void applyTorque(const Vector3& torque);
/// Add a collision shape to the body.
const CollisionShape* addCollisionShape(const CollisionShape& collisionShape,
decimal mass,
const Transform& transform = Transform::identity());
const ProxyShape* addCollisionShape(const CollisionShape& collisionShape,
decimal mass,
const Transform& transform = Transform::identity());
/// Remove a collision shape from the body
virtual void removeCollisionShape(const CollisionShape* collisionShape);
virtual void removeCollisionShape(const ProxyShape* proxyCollisionShape);
/// Recompute the center of mass, total mass and inertia tensor of the body using all
/// the collision shapes attached to the body.

96
src/collision/CollisionDetection.cpp
View File

@ -26,8 +26,6 @@
// Libraries
#include "CollisionDetection.h"
#include "../engine/CollisionWorld.h"
#include "broadphase/SweepAndPruneAlgorithm.h"
#include "broadphase/NoBroadPhaseAlgorithm.h"
#include "../body/Body.h"
#include "../collision/shapes/BoxShape.h"
#include "../body/RigidBody.h"
@ -101,7 +99,7 @@ void CollisionDetection::computeNarrowPhase() {
CollisionBody* const body2 = shape2->getBody();
// Update the contact cache of the overlapping pair
mWorld->updateOverlappingPair(pair);
pair->update();
// Check if the two bodies are allowed to collide, otherwise, we do not test for collision
if (body1->getType() != DYNAMIC && body2->getType() != DYNAMIC) continue;
@ -123,9 +121,7 @@ void CollisionDetection::computeNarrowPhase() {
// if there really is a collision
const Transform transform1 = body1->getTransform() * shape1->getLocalToBodyTransform();
const Transform transform2 = body2->getTransform() * shape2->getLocalToBodyTransform();
if (narrowPhaseAlgorithm.testCollision(shape1->getCollisionShape(), transform1,
shape2->getCollisionShape(), transform2,
contactInfo)) {
if (narrowPhaseAlgorithm.testCollision(shape1, shape2, contactInfo)) {
assert(contactInfo != NULL);
// Set the bodies of the contact
@ -134,8 +130,8 @@ void CollisionDetection::computeNarrowPhase() {
assert(contactInfo->body1 != NULL);
assert(contactInfo->body2 != NULL);
// Notify the world about the new narrow-phase contact
mWorld->notifyNewContact(pair, contactInfo);
// Create a new contact
createContact(pair, contactInfo);
// Delete and remove the contact info from the memory allocator
contactInfo->ContactPointInfo::~ContactPointInfo();
@ -185,37 +181,20 @@ void CollisionDetection::broadPhaseNotifyOverlappingPair(ProxyShape* shape1, Pro
*/
}
// Allow the broadphase to notify the collision detection about a removed overlapping pair
void CollisionDetection::removeOverlappingPair(ProxyShape* shape1, ProxyShape* shape2) {
// Compute the overlapping pair ID
overlappingpairid pairID = OverlappingPair::computeID(shape1, shape2);
// If the overlapping
std::map<overlappingpairid, OverlappingPair*>::iterator it;
it = mOverlappingPairs.find(indexPair);
if ()
// Notify the world about the removed broad-phase pair
// TODO : DELETE THIS
//mWorld->notifyRemovedOverlappingPair(broadPhasePair);
// Remove the overlapping pair from the memory allocator
mOverlappingPairs.find(pairID)->second->OverlappingPair::~OverlappingPair();
mWorld->mMemoryAllocator.release(mOverlappingPairs[indexPair], sizeof(OverlappingPair));
mOverlappingPairs.erase(pairID);
}
// Remove a body from the collision detection
void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
// Remove all the overlapping pairs involving this proxy shape
std::map<overlappingpairid, OverlappingPair*>::iterator it;
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
if (it->second->getShape1()->getBroadPhaseID() == proxyShape->getBroadPhaseID() ||
it->second->getShape2()->getBroadPhaseID() == proxyShape->getBroadPhaseID()) {
if (it->second->getShape1()->mBroadPhaseID == proxyShape->mBroadPhaseID||
it->second->getShape2()->mBroadPhaseID == proxyShape->mBroadPhaseID) {
std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
++it;
// Destroy the overlapping pair
itToRemove->second->OverlappingPair::~OverlappingPair();
mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair));
mOverlappingPairs.erase(itToRemove);
}
else {
@ -226,3 +205,58 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
// Remove the body from the broad-phase
mBroadPhaseAlgorithm.removeProxyCollisionShape(proxyShape);
}
// Create a new contact
void CollisionDetection::createContact(OverlappingPair* overlappingPair,
const ContactPointInfo* contactInfo) {
// Create a new contact
ContactPoint* contact = new (mWorld->mMemoryAllocator.allocate(sizeof(ContactPoint)))
ContactPoint(*contactInfo);
assert(contact != NULL);
// If it is the first contact since the pair are overlapping
if (overlappingPair->getNbContactPoints() == 0) {
// Trigger a callback event
if (mWorld->mEventListener != NULL) mWorld->mEventListener->beginContact(*contactInfo);
}
// Add the contact to the contact cache of the corresponding overlapping pair
overlappingPair->addContact(contact);
// Add the contact manifold to the list of contact manifolds
mContactManifolds.push_back(overlappingPair->getContactManifold());
// 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());
// Trigger a callback event for the new contact
if (mWorld->mEventListener != NULL) mWorld->mEventListener->newContact(*contactInfo);
}
// 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) {
assert(contactManifold != 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;
// Add the joint at the beginning of the linked list of joints of the second body
void* allocatedMemory2 = mWorld->mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement2 = new (allocatedMemory2)
ContactManifoldListElement(contactManifold,
body2->mContactManifoldsList);
body2->mContactManifoldsList = listElement2;
}

18
src/collision/CollisionDetection.h
View File

@ -80,6 +80,10 @@ class CollisionDetection {
/// True if some collision shapes have been added previously
bool mIsCollisionShapesAdded;
/// All the contact constraints
// TODO : Remove this variable (we will use the ones in the island now)
std::vector<ContactManifold*> mContactManifolds;
// -------------------- Methods -------------------- //
/// Private copy-constructor
@ -98,8 +102,13 @@ class CollisionDetection {
NarrowPhaseAlgorithm& SelectNarrowPhaseAlgorithm(const CollisionShape* collisionShape1,
const CollisionShape* collisionShape2);
/// Remove an overlapping pair if it is not overlapping anymore
void removeOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
/// 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);
public :
@ -131,6 +140,11 @@ class CollisionDetection {
/// Allow the broadphase to notify the collision detection about an overlapping pair.
void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
// -------------------- Friendship -------------------- //
// TODO : REMOVE THIS
friend class DynamicsWorld;
};
// Select the narrow-phase collision algorithm to use given two collision shapes

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

@ -25,6 +25,7 @@
// Libraries
#include "BroadPhaseAlgorithm.h"
#include "../CollisionDetection.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
@ -33,7 +34,7 @@ using namespace reactphysics3d;
BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
:mDynamicAABBTree(*this), mNbMovedShapes(0), mNbAllocatedMovedShapes(8),
mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8),
mPairManager(collisionDetection), mCollisionDetection(collisionDetection) {
mCollisionDetection(collisionDetection) {
// Allocate memory for the array of non-static bodies IDs
mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));

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

@ -29,7 +29,6 @@
// Libraries
#include <vector>
#include "../../body/CollisionBody.h"
#include "PairManager.h"
#include "DynamicAABBTree.h"
/// Namespace ReactPhysics3D
@ -110,9 +109,6 @@ class BroadPhaseAlgorithm {
/// Number of allocated elements for the array of potential overlapping pairs
uint mNbAllocatedPotentialPairs;
/// Pair manager containing the overlapping pairs
PairManager mPairManager;
/// Reference to the collision detection object
CollisionDetection& mCollisionDetection;
@ -156,14 +152,6 @@ class BroadPhaseAlgorithm {
/// Compute all the overlapping pairs of collision shapes
void computeOverlappingPairs();
/// Return a pointer to the first active pair (used to iterate over the active pairs)
// TODO : DELETE THIS
BodyPair* beginOverlappingPairsPointer() const;
/// Return a pointer to the last active pair (used to iterate over the active pairs)
// TODO : DELETE THIS
BodyPair* endOverlappingPairsPointer() const;
};
// Method used to compare two pairs for sorting algorithm
@ -176,16 +164,6 @@ inline bool BroadPair::smallerThan(const BroadPair& pair1, const BroadPair& pair
return false;
}
// Return a pointer to the first active pair (used to iterate over the overlapping pairs)
inline BodyPair* BroadPhaseAlgorithm::beginOverlappingPairsPointer() const {
return mPairManager.beginOverlappingPairsPointer();
}
// Return a pointer to the last active pair (used to iterate over the overlapping pairs)
inline BodyPair* BroadPhaseAlgorithm::endOverlappingPairsPointer() const {
return mPairManager.endOverlappingPairsPointer();
}
}
#endif

43
src/collision/broadphase/NoBroadPhaseAlgorithm.cpp
View File

@ -1,43 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 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 "NoBroadPhaseAlgorithm.h"
#include "../CollisionDetection.h"
// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;
using namespace std;
// Constructor
NoBroadPhaseAlgorithm::NoBroadPhaseAlgorithm(CollisionDetection& collisionDetection)
: BroadPhaseAlgorithm(collisionDetection) {
}
// Destructor
NoBroadPhaseAlgorithm::~NoBroadPhaseAlgorithm() {
}

123
src/collision/broadphase/NoBroadPhaseAlgorithm.h
View File

@ -1,123 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 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_NO_BROAD_PHASE_ALGORITHM_H
#define REACTPHYSICS3D_NO_BROAD_PHASE_ALGORITHM_H
// Libraries
#include "BroadPhaseAlgorithm.h"
#include <algorithm>
#include <set>
#include <iostream>
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Class NoBroadPhaseAlgorithm
/**
* This class implements a broad-phase algorithm that does nothing.
* It should be use if we don't want to perform a broad-phase for
* the collision detection.
*/
class NoBroadPhaseAlgorithm : public BroadPhaseAlgorithm {
protected :
// -------------------- Attributes -------------------- //
/// All bodies of the world
std::set<CollisionBody*> mBodies;
// -------------------- Methods -------------------- //
/// Private copy-constructor
NoBroadPhaseAlgorithm(const NoBroadPhaseAlgorithm& algorithm);
/// Private assignment operator
NoBroadPhaseAlgorithm& operator=(const NoBroadPhaseAlgorithm& algorithm);
public :
// -------------------- Methods -------------------- //
/// Constructor
NoBroadPhaseAlgorithm(CollisionDetection& collisionDetection);
/// Destructor
virtual ~NoBroadPhaseAlgorithm();
/// Notify the broad-phase about a new object in the world
virtual void addProxyCollisionShape(CollisionBody* body, const AABB& aabb);
/// Notify the broad-phase about an object that has been removed from the world
virtual void removeProxyCollisionShape(CollisionBody* body);
/// Notify the broad-phase that the AABB of an object has changed
virtual void updateProxyCollisionShape(CollisionBody* body, const AABB& aabb);
};
// Notify the broad-phase about a new object in the world
inline void NoBroadPhaseAlgorithm::addProxyCollisionShape(CollisionBody* body, const AABB& aabb) {
// For each body that is already in the world
for (std::set<CollisionBody*>::iterator it = mBodies.begin(); it != mBodies.end(); ++it) {
// Add an overlapping pair with the new body
mPairManager.addPair(*it, body);
}
// Add the new body into the list of bodies
mBodies.insert(body);
}
// Notify the broad-phase about an object that has been removed from the world
inline void NoBroadPhaseAlgorithm::removeProxyCollisionShape(CollisionBody* body) {
// For each body that is in the world
for (std::set<CollisionBody*>::iterator it = mBodies.begin(); it != mBodies.end(); ++it) {
if ((*it)->getID() != body->getID()) {
// Remove the overlapping pair with the new body
mPairManager.removePair((*it)->getID(), body->getID());
}
}
// Remove the body from the broad-phase
mBodies.erase(body);
}
// Notify the broad-phase that the AABB of an object has changed
inline void NoBroadPhaseAlgorithm::updateProxyCollisionShape(CollisionBody* body, const AABB& aabb) {
// Do nothing
return;
}
}
#endif

596
src/collision/broadphase/SweepAndPruneAlgorithm.cpp
View File

@ -1,596 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 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 "SweepAndPruneAlgorithm.h"
#include "../CollisionDetection.h"
#include "PairManager.h"
#include <climits>
#include <cstring>
// Namespaces
using namespace reactphysics3d;
using namespace std;
// Initialization of static variables
const bodyindex SweepAndPruneAlgorithm::INVALID_INDEX = std::numeric_limits<bodyindex>::max();
const luint SweepAndPruneAlgorithm::NB_SENTINELS = 2;
// Constructor that takes an AABB as input
AABBInt::AABBInt(const AABB& aabb) {
min[0] = encodeFloatIntoInteger(aabb.getMin().x);
min[1] = encodeFloatIntoInteger(aabb.getMin().y);
min[2] = encodeFloatIntoInteger(aabb.getMin().z);
max[0] = encodeFloatIntoInteger(aabb.getMax().x);
max[1] = encodeFloatIntoInteger(aabb.getMax().y);
max[2] = encodeFloatIntoInteger(aabb.getMax().z);
}
// Constructor that set all the axis with an minimum and maximum value
AABBInt::AABBInt(uint minValue, uint maxValue) {
min[0] = minValue;
min[1] = minValue;
min[2] = minValue;
max[0] = maxValue;
max[1] = maxValue;
max[2] = maxValue;
}
// Constructor
SweepAndPruneAlgorithm::SweepAndPruneAlgorithm(CollisionDetection& collisionDetection)
:BroadPhaseAlgorithm(collisionDetection) {
mBoxes = NULL;
mEndPoints[0] = NULL;
mEndPoints[1] = NULL;
mEndPoints[2] = NULL;
mNbBoxes = 0;
mNbMaxBoxes = 0;
}
// Destructor
SweepAndPruneAlgorithm::~SweepAndPruneAlgorithm() {
delete[] mBoxes;
delete[] mEndPoints[0];
delete[] mEndPoints[1];
delete[] mEndPoints[2];
}
// Notify the broad-phase about a new object in the world
/// This method adds the AABB of the object ion to broad-phase
void SweepAndPruneAlgorithm::addProxyCollisionShape(CollisionBody* body, const AABB& aabb) {
bodyindex boxIndex;
assert(encodeFloatIntoInteger(aabb.getMin().x) > encodeFloatIntoInteger(-FLT_MAX));
assert(encodeFloatIntoInteger(aabb.getMin().y) > encodeFloatIntoInteger(-FLT_MAX));
assert(encodeFloatIntoInteger(aabb.getMin().z) > encodeFloatIntoInteger(-FLT_MAX));
assert(encodeFloatIntoInteger(aabb.getMax().x) < encodeFloatIntoInteger(FLT_MAX));
assert(encodeFloatIntoInteger(aabb.getMax().y) < encodeFloatIntoInteger(FLT_MAX));
assert(encodeFloatIntoInteger(aabb.getMax().z) < encodeFloatIntoInteger(FLT_MAX));
// If the index of the first free box is valid (means that
// there is a bucket in the middle of the array that doesn't
// contain a box anymore because it has been removed)
if (!mFreeBoxIndices.empty()) {
boxIndex = mFreeBoxIndices.back();
mFreeBoxIndices.pop_back();
}
else {
// If the array boxes and end-points arrays are full
if (mNbBoxes == mNbMaxBoxes) {
// Resize the arrays to make them larger
resizeArrays();
}
boxIndex = mNbBoxes;
}
// Move the maximum limit end-point two elements further
// at the end-points array in all three axis
const luint indexLimitEndPoint = 2 * mNbBoxes + NB_SENTINELS - 1;
for (uint axis=0; axis<3; axis++) {
EndPoint* maxLimitEndPoint = &mEndPoints[axis][indexLimitEndPoint];
assert(mEndPoints[axis][0].boxID == INVALID_INDEX && mEndPoints[axis][0].isMin);
assert(maxLimitEndPoint->boxID == INVALID_INDEX && !maxLimitEndPoint->isMin);
EndPoint* newMaxLimitEndPoint = &mEndPoints[axis][indexLimitEndPoint + 2];
newMaxLimitEndPoint->setValues(maxLimitEndPoint->boxID, maxLimitEndPoint->isMin,
maxLimitEndPoint->value);
}
// Create a new box
BoxAABB* box = &mBoxes[boxIndex];
box->body = body;
const uint maxEndPointValue = encodeFloatIntoInteger(FLT_MAX) - 1;
const uint minEndPointValue = encodeFloatIntoInteger(FLT_MAX) - 2;
for (uint axis=0; axis<3; axis++) {
box->min[axis] = indexLimitEndPoint;
box->max[axis] = indexLimitEndPoint + 1;
EndPoint* minimumEndPoint = &mEndPoints[axis][box->min[axis]];
minimumEndPoint->setValues(boxIndex, true, minEndPointValue);
EndPoint* maximumEndPoint = &mEndPoints[axis][box->max[axis]];
maximumEndPoint->setValues(boxIndex, false, maxEndPointValue);
}
// Add the body pointer to box index mapping
mMapBodyToBoxIndex.insert(pair<CollisionBody*, bodyindex>(body, boxIndex));
mNbBoxes++;
// Call the update method to put the end-points of the new AABB at the
// correct position in the array. This will also create the overlapping
// pairs in the pair manager if the new AABB is overlapping with others
// AABBs
updateProxyCollisionShape(body, aabb);
}
// Notify the broad-phase about an object that has been removed from the world
void SweepAndPruneAlgorithm::removeProxyCollisionShape(CollisionBody* body) {
assert(mNbBoxes > 0);
// Call the update method with an AABB that is very far away
// in order to remove all overlapping pairs from the pair manager
const uint maxEndPointValue = encodeFloatIntoInteger(FLT_MAX) - 1;
const uint minEndPointValue = encodeFloatIntoInteger(FLT_MAX) - 2;
const AABBInt aabbInt(minEndPointValue, maxEndPointValue);
updateObjectIntegerAABB(body, aabbInt);
// Get the corresponding box
bodyindex boxIndex = mMapBodyToBoxIndex.find(body)->second;
// Remove the end-points of the box by moving the maximum end-points two elements back in
// the end-points array
const luint indexLimitEndPoint = 2 * mNbBoxes + NB_SENTINELS - 1;
for (uint axis=0; axis < 3; axis++) {
EndPoint* maxLimitEndPoint = &mEndPoints[axis][indexLimitEndPoint];
assert(mEndPoints[axis][0].boxID == INVALID_INDEX && mEndPoints[axis][0].isMin);
assert(maxLimitEndPoint->boxID == INVALID_INDEX && !maxLimitEndPoint->isMin);
EndPoint* newMaxLimitEndPoint = &mEndPoints[axis][indexLimitEndPoint - 2];
assert(mEndPoints[axis][indexLimitEndPoint - 1].boxID == boxIndex);
assert(!mEndPoints[axis][indexLimitEndPoint - 1].isMin);
assert(newMaxLimitEndPoint->boxID == boxIndex);
assert(newMaxLimitEndPoint->isMin);
newMaxLimitEndPoint->setValues(maxLimitEndPoint->boxID, maxLimitEndPoint->isMin,
maxLimitEndPoint->value);
}
// Add the box index into the list of free indices
mFreeBoxIndices.push_back(boxIndex);
mMapBodyToBoxIndex.erase(body);
mNbBoxes--;
// Check if we need to shrink the allocated memory
const luint nextPowerOf2 = PairManager::computeNextPowerOfTwo((mNbBoxes-1) / 100 );
if (nextPowerOf2 * 100 < mNbMaxBoxes) {
shrinkArrays();
}
}
// Notify the broad-phase that the AABB of an object has changed.
/// The input is an AABB with integer coordinates
void SweepAndPruneAlgorithm::updateObjectIntegerAABB(CollisionBody* body, const AABBInt& aabbInt) {
assert(aabbInt.min[0] > encodeFloatIntoInteger(-FLT_MAX));
assert(aabbInt.min[1] > encodeFloatIntoInteger(-FLT_MAX));
assert(aabbInt.min[2] > encodeFloatIntoInteger(-FLT_MAX));
assert(aabbInt.max[0] < encodeFloatIntoInteger(FLT_MAX));
assert(aabbInt.max[1] < encodeFloatIntoInteger(FLT_MAX));
assert(aabbInt.max[2] < encodeFloatIntoInteger(FLT_MAX));
// Get the corresponding box
bodyindex boxIndex = mMapBodyToBoxIndex.find(body)->second;
BoxAABB* box = &mBoxes[boxIndex];
// Current axis
for (uint axis=0; axis<3; axis++) {
// Get the two others axis
const uint otherAxis1 = (1 << axis) & 3;
const uint otherAxis2 = (1 << otherAxis1) & 3;
// Get the starting end-point of the current axis
EndPoint* startEndPointsCurrentAxis = mEndPoints[axis];
// -------- Update the minimum end-point ------------//
EndPoint* currentMinEndPoint = &startEndPointsCurrentAxis[box->min[axis]];
assert(currentMinEndPoint->isMin);
// Get the minimum value of the AABB on the current axis
uint limit = aabbInt.min[axis];
// If the minimum value of the AABB is smaller
// than the current minimum endpoint
if (limit < currentMinEndPoint->value) {
currentMinEndPoint->value = limit;
// The minimum end-point is moving left
EndPoint savedEndPoint = *currentMinEndPoint;
luint indexEndPoint = (size_t(currentMinEndPoint) -
size_t(startEndPointsCurrentAxis)) / sizeof(EndPoint);
const luint savedEndPointIndex = indexEndPoint;
while ((--currentMinEndPoint)->value > limit) {
BoxAABB* id1 = &mBoxes[currentMinEndPoint->boxID];
const bool isMin = currentMinEndPoint->isMin;
// If it's a maximum end-point
if (!isMin) {
// The minimum end-point is moving to the left and
// passed a maximum end-point. Thus, the boxes start
// overlapping on the current axis. Therefore we test
// for box intersection
if (box != id1) {
if (testIntersect2D(*box, *id1, otherAxis1, otherAxis2) &&
testIntersect1DSortedAABBs(*id1, aabbInt,
startEndPointsCurrentAxis, axis)) {
// Add an overlapping pair to the pair manager
mPairManager.addPair(body, id1->body);
}
}
id1->max[axis] = indexEndPoint--;
}
else {
id1->min[axis] = indexEndPoint--;
}
*(currentMinEndPoint+1) = *currentMinEndPoint;
}
// Update the current minimum endpoint that we are moving
if (savedEndPointIndex != indexEndPoint) {
if (savedEndPoint.isMin) {
mBoxes[savedEndPoint.boxID].min[axis] = indexEndPoint;
}
else {
mBoxes[savedEndPoint.boxID].max[axis] = indexEndPoint;
}
startEndPointsCurrentAxis[indexEndPoint] = savedEndPoint;
}
}
else if (limit > currentMinEndPoint->value){// The minimum of the box has moved to the right
currentMinEndPoint->value = limit;
// The minimum en-point is moving right
EndPoint savedEndPoint = *currentMinEndPoint;
luint indexEndPoint = (size_t(currentMinEndPoint) -
size_t(startEndPointsCurrentAxis)) / sizeof(EndPoint);
const luint savedEndPointIndex = indexEndPoint;
// For each end-point between the current position of the minimum
// end-point and the new position of the minimum end-point
while ((++currentMinEndPoint)->value < limit) {
BoxAABB* id1 = &mBoxes[currentMinEndPoint->boxID];
const bool isMin = currentMinEndPoint->isMin;
// If it's a maximum end-point
if (!isMin) {
// The minimum end-point is moving to the right and
// passed a maximum end-point. Thus, the boxes stop
// overlapping on the current axis.
if (box != id1) {
if (testIntersect2D(*box, *id1, otherAxis1, otherAxis2)) {
// Remove the pair from the pair manager
mPairManager.removePair(body->getID(), id1->body->getID());
}
}
id1->max[axis] = indexEndPoint++;
}
else {
id1->min[axis] = indexEndPoint++;
}
*(currentMinEndPoint-1) = *currentMinEndPoint;
}
// Update the current minimum endpoint that we are moving
if (savedEndPointIndex != indexEndPoint) {
if (savedEndPoint.isMin) {
mBoxes[savedEndPoint.boxID].min[axis] = indexEndPoint;
}
else {
mBoxes[savedEndPoint.boxID].max[axis] = indexEndPoint;
}
startEndPointsCurrentAxis[indexEndPoint] = savedEndPoint;
}
}
// ------- Update the maximum end-point ------------ //
EndPoint* currentMaxEndPoint = &startEndPointsCurrentAxis[box->max[axis]];
assert(!currentMaxEndPoint->isMin);
// Get the maximum value of the AABB on the current axis
limit = aabbInt.max[axis];
// If the new maximum value of the AABB is larger
// than the current maximum end-point value. It means
// that the AABB is moving to the right.
if (limit > currentMaxEndPoint->value) {
currentMaxEndPoint->value = limit;
EndPoint savedEndPoint = *currentMaxEndPoint;
luint indexEndPoint = (size_t(currentMaxEndPoint) -
size_t(startEndPointsCurrentAxis)) / sizeof(EndPoint);
const luint savedEndPointIndex = indexEndPoint;
while ((++currentMaxEndPoint)->value < limit) {
// Get the next end-point
BoxAABB* id1 = &mBoxes[currentMaxEndPoint->boxID];
const bool isMin = currentMaxEndPoint->isMin;
// If it's a maximum end-point
if (isMin) {
// The maximum end-point is moving to the right and
// passed a minimum end-point. Thus, the boxes start
// overlapping on the current axis. Therefore we test
// for box intersection
if (box != id1) {
if (testIntersect2D(*box, *id1, otherAxis1, otherAxis2) &&
testIntersect1DSortedAABBs(*id1, aabbInt,
startEndPointsCurrentAxis,axis)) {
// Add an overlapping pair to the pair manager
mPairManager.addPair(body, id1->body);
}
}
id1->min[axis] = indexEndPoint++;
}
else {
id1->max[axis] = indexEndPoint++;
}
*(currentMaxEndPoint-1) = *currentMaxEndPoint;
}
// Update the current minimum endpoint that we are moving
if (savedEndPointIndex != indexEndPoint) {
if (savedEndPoint.isMin) {
mBoxes[savedEndPoint.boxID].min[axis] = indexEndPoint;
}
else {
mBoxes[savedEndPoint.boxID].max[axis] = indexEndPoint;
}
startEndPointsCurrentAxis[indexEndPoint] = savedEndPoint;
}
}
else if (limit < currentMaxEndPoint->value) { // If the AABB is moving to the left
currentMaxEndPoint->value = limit;
EndPoint savedEndPoint = *currentMaxEndPoint;
luint indexEndPoint = (size_t(currentMaxEndPoint) -
size_t(startEndPointsCurrentAxis)) / sizeof(EndPoint);
const luint savedEndPointIndex = indexEndPoint;
// For each end-point between the current position of the maximum
// end-point and the new position of the maximum end-point
while ((--currentMaxEndPoint)->value > limit) {
BoxAABB* id1 = &mBoxes[currentMaxEndPoint->boxID];
const bool isMin = currentMaxEndPoint->isMin;
// If it's a minimum end-point
if (isMin) {
// The maximum end-point is moving to the right and
// passed a minimum end-point. Thus, the boxes stop
// overlapping on the current axis.
if (box != id1) {
if (testIntersect2D(*box, *id1, otherAxis1, otherAxis2)) {
// Remove the pair from the pair manager
mPairManager.removePair(body->getID(), id1->body->getID());
}
}
id1->min[axis] = indexEndPoint--;
}
else {
id1->max[axis] = indexEndPoint--;
}
*(currentMaxEndPoint+1) = *currentMaxEndPoint;
}
// Update the current minimum endpoint that we are moving
if (savedEndPointIndex != indexEndPoint) {
if (savedEndPoint.isMin) {
mBoxes[savedEndPoint.boxID].min[axis] = indexEndPoint;
}
else {
mBoxes[savedEndPoint.boxID].max[axis] = indexEndPoint;
}
startEndPointsCurrentAxis[indexEndPoint] = savedEndPoint;
}
}
}
}
// Resize the boxes and end-points arrays when it is full
void SweepAndPruneAlgorithm::resizeArrays() {
// New number of boxes in the array
const luint newNbMaxBoxes = mNbMaxBoxes ? 2 * mNbMaxBoxes : 100;
const luint nbEndPoints = mNbBoxes * 2 + NB_SENTINELS;
const luint newNbEndPoints = newNbMaxBoxes * 2 + NB_SENTINELS;
// Allocate memory for the new boxes and end-points arrays
BoxAABB* newBoxesArray = new BoxAABB[newNbMaxBoxes];
EndPoint* newEndPointsXArray = new EndPoint[newNbEndPoints];
EndPoint* newEndPointsYArray = new EndPoint[newNbEndPoints];
EndPoint* newEndPointsZArray = new EndPoint[newNbEndPoints];
assert(newBoxesArray != NULL);
assert(newEndPointsXArray != NULL);
assert(newEndPointsYArray != NULL);
assert(newEndPointsZArray != NULL);
// If the arrays were not empty before
if (mNbBoxes > 0) {
// Copy the data from the old arrays into the new one
memcpy(newBoxesArray, mBoxes, sizeof(BoxAABB) * mNbBoxes);
const size_t nbBytesNewEndPoints = sizeof(EndPoint) * nbEndPoints;
memcpy(newEndPointsXArray, mEndPoints[0], nbBytesNewEndPoints);
memcpy(newEndPointsYArray, mEndPoints[1], nbBytesNewEndPoints);
memcpy(newEndPointsZArray, mEndPoints[2], nbBytesNewEndPoints);
}
else { // If the arrays were empty
// Add the limits endpoints (sentinels) into the array
const uint min = encodeFloatIntoInteger(-FLT_MAX);
const uint max = encodeFloatIntoInteger(FLT_MAX);
newEndPointsXArray[0].setValues(INVALID_INDEX, true, min);
newEndPointsXArray[1].setValues(INVALID_INDEX, false, max);
newEndPointsYArray[0].setValues(INVALID_INDEX, true, min);
newEndPointsYArray[1].setValues(INVALID_INDEX, false, max);
newEndPointsZArray[0].setValues(INVALID_INDEX, true, min);
newEndPointsZArray[1].setValues(INVALID_INDEX, false, max);
}
// Delete the old arrays
delete[] mBoxes;
delete[] mEndPoints[0];
delete[] mEndPoints[1];
delete[] mEndPoints[2];
// Assign the pointer to the new arrays
mBoxes = newBoxesArray;
mEndPoints[0] = newEndPointsXArray;
mEndPoints[1] = newEndPointsYArray;
mEndPoints[2] = newEndPointsZArray;
mNbMaxBoxes = newNbMaxBoxes;
}
// Shrink the boxes and end-points arrays when too much memory is allocated
void SweepAndPruneAlgorithm::shrinkArrays() {
// New number of boxes and end-points in the array
const luint nextPowerOf2 = PairManager::computeNextPowerOfTwo((mNbBoxes-1) / 100 );
const luint newNbMaxBoxes = (mNbBoxes > 100) ? nextPowerOf2 * 100 : 100;
const luint nbEndPoints = mNbBoxes * 2 + NB_SENTINELS;
const luint newNbEndPoints = newNbMaxBoxes * 2 + NB_SENTINELS;
assert(newNbMaxBoxes < mNbMaxBoxes);
// Sort the list of the free boxes indices in ascending order
mFreeBoxIndices.sort();
// Reorganize the boxes inside the boxes array so that all the boxes are at the
// beginning of the array
std::map<CollisionBody*, bodyindex> newMapBodyToBoxIndex;
std::map<CollisionBody*,bodyindex>::const_iterator it;
for (it = mMapBodyToBoxIndex.begin(); it != mMapBodyToBoxIndex.end(); ++it) {
CollisionBody* body = it->first;
bodyindex boxIndex = it->second;
// If the box index is outside the range of the current number of boxes
if (boxIndex >= mNbBoxes) {
assert(!mFreeBoxIndices.empty());
// Get a new box index for that body (from the list of free box indices)
bodyindex newBoxIndex = mFreeBoxIndices.front();
mFreeBoxIndices.pop_front();
assert(newBoxIndex < mNbBoxes);
// Copy the box to its new location in the boxes array
BoxAABB* oldBox = &mBoxes[boxIndex];
BoxAABB* newBox = &mBoxes[newBoxIndex];
assert(oldBox->body->getID() == body->getID());
newBox->body = oldBox->body;
for (uint axis=0; axis<3; axis++) {
// Copy the minimum and maximum end-points indices
newBox->min[axis] = oldBox->min[axis];
newBox->max[axis] = oldBox->max[axis];
// Update the box index of the end-points
EndPoint* minimumEndPoint = &mEndPoints[axis][newBox->min[axis]];
EndPoint* maximumEndPoint = &mEndPoints[axis][newBox->max[axis]];
assert(minimumEndPoint->boxID == boxIndex);
assert(maximumEndPoint->boxID == boxIndex);
minimumEndPoint->boxID = newBoxIndex;
maximumEndPoint->boxID = newBoxIndex;
}
newMapBodyToBoxIndex.insert(pair<CollisionBody*, bodyindex>(body, newBoxIndex));
}
else {
newMapBodyToBoxIndex.insert(pair<CollisionBody*, bodyindex>(body, boxIndex));
}
}
assert(newMapBodyToBoxIndex.size() == mMapBodyToBoxIndex.size());
mMapBodyToBoxIndex = newMapBodyToBoxIndex;
// Allocate memory for the new boxes and end-points arrays
BoxAABB* newBoxesArray = new BoxAABB[newNbMaxBoxes];
EndPoint* newEndPointsXArray = new EndPoint[newNbEndPoints];
EndPoint* newEndPointsYArray = new EndPoint[newNbEndPoints];
EndPoint* newEndPointsZArray = new EndPoint[newNbEndPoints];
assert(newBoxesArray != NULL);
assert(newEndPointsXArray != NULL);
assert(newEndPointsYArray != NULL);
assert(newEndPointsZArray != NULL);
// Copy the data from the old arrays into the new one
memcpy(newBoxesArray, mBoxes, sizeof(BoxAABB) * mNbBoxes);
const size_t nbBytesNewEndPoints = sizeof(EndPoint) * nbEndPoints;
memcpy(newEndPointsXArray, mEndPoints[0], nbBytesNewEndPoints);
memcpy(newEndPointsYArray, mEndPoints[1], nbBytesNewEndPoints);
memcpy(newEndPointsZArray, mEndPoints[2], nbBytesNewEndPoints);
// Delete the old arrays
delete[] mBoxes;
delete[] mEndPoints[0];
delete[] mEndPoints[1];
delete[] mEndPoints[2];
// Assign the pointer to the new arrays
mBoxes = newBoxesArray;
mEndPoints[0] = newEndPointsXArray;
mEndPoints[1] = newEndPointsYArray;
mEndPoints[2] = newEndPointsZArray;
mNbMaxBoxes = newNbMaxBoxes;
}

245
src/collision/broadphase/SweepAndPruneAlgorithm.h
View File

@ -1,245 +0,0 @@
/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010-2013 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_SWEEP_AND_PRUNE_ALGORITHM_H
#define REACTPHYSICS3D_SWEEP_AND_PRUNE_ALGORITHM_H
// Libraries
#include "BroadPhaseAlgorithm.h"
#include "../../collision/shapes/AABB.h"
#include <map>
#include <vector>
#include <list>
/// Namespace ReactPhysics3D
namespace reactphysics3d {
// Structure EndPoint
/**
* EndPoint structure that represent an end-point of an AABB
* on one of the three x,y or z axis.
*/
struct EndPoint {
public:
/// ID of the AABB box corresponding to this end-point
bodyindex boxID;
/// True if the end-point is a minimum end-point of a box
bool isMin;
/// Value (one dimension coordinate) of the end-point
uint value;
/// Set the values of the endpoint
void setValues(bodyindex boxID, bool isMin, uint value) {
this->boxID = boxID;
this->isMin = isMin;
this->value = value;
}
};
// Structure BoxAABB
/**
* This structure represents an AABB in the Sweep-And-Prune algorithm
*/
struct BoxAABB {
public:
/// Index of the 3 minimum end-points of the AABB over the x,y,z axis
bodyindex min[3];
/// Index of the 3 maximum end-points of the AABB over the x,y,z axis
bodyindex max[3];
/// Body that corresponds to the owner of the AABB
CollisionBody* body;
};
// Structure AABBInt
/**
* Axis-Aligned Bounding box with integer coordinates.
*/
struct AABBInt {
public:
/// Minimum values on the three axis
uint min[3];
/// Maximum values on the three axis
uint max[3];
/// Constructor that takes an AABB as input
AABBInt(const AABB& aabb);
/// Constructor that set all the axis with an minimum and maximum value
AABBInt(uint minValue, uint maxValue);
};
// Class SweepAndPruneAlgorithm
/**
* This class implements the Sweep-And-Prune (SAP) broad-phase
* collision detection algorithm. This class implements an
* array-based implementation of the algorithm from Pierre Terdiman
* that is described here : www.codercorner.com/SAP.pdf.
*/
class SweepAndPruneAlgorithm : public BroadPhaseAlgorithm {
protected :
// -------------------- Constants -------------------- //
/// Invalid array index
const static bodyindex INVALID_INDEX;
/// Number of sentinel end-points in the array of a given axis
const static luint NB_SENTINELS;
// -------------------- Attributes -------------------- //
/// Array that contains all the AABB boxes of the broad-phase
BoxAABB* mBoxes;
/// Array of end-points on the three axis
EndPoint* mEndPoints[3];
/// Number of AABB boxes in the broad-phase
bodyindex mNbBoxes;
/// Max number of boxes in the boxes array
bodyindex mNbMaxBoxes;
/// Indices that are not used by any boxes
std::list<bodyindex> mFreeBoxIndices;
/// Map a body pointer to a box index
std::map<CollisionBody*,bodyindex> mMapBodyToBoxIndex;
// -------------------- Methods -------------------- //
/// Private copy-constructor
SweepAndPruneAlgorithm(const SweepAndPruneAlgorithm& algorithm);
/// Private assignment operator
SweepAndPruneAlgorithm& operator=(const SweepAndPruneAlgorithm& algorithm);
/// Resize the boxes and end-points arrays when it's full
void resizeArrays();
/// Shrink the boxes and end-points arrays when too much memory is allocated
void shrinkArrays();
/// Add an overlapping pair of AABBS
void addPair(CollisionBody* body1, CollisionBody* body2);
/// Check for 1D box intersection between two boxes that are sorted on the given axis.
bool testIntersect1DSortedAABBs(const BoxAABB& box1, const AABBInt& box2,
const EndPoint* const baseEndPoint, uint axis) const;
/// Check for 2D box intersection.
bool testIntersect2D(const BoxAABB& box1, const BoxAABB& box2,
luint axis1, uint axis2) const;
/// Notify the broad-phase that the AABB of an object has changed.
void updateObjectIntegerAABB(CollisionBody* body, const AABBInt& aabbInt);
public :
// -------------------- Methods -------------------- //
/// Constructor
SweepAndPruneAlgorithm(CollisionDetection& mCollisionDetection);
/// Destructor
virtual ~SweepAndPruneAlgorithm();
/// Notify the broad-phase about a new object in the world.
virtual void addProxyCollisionShape(CollisionBody* body, const AABB& aabb);
/// Notify the broad-phase about a object that has been removed from the world
virtual void removeProxyCollisionShape(CollisionBody* body);
/// Notify the broad-phase that the AABB of an object has changed
virtual void updateProxyCollisionShape(CollisionBody* body, const AABB& aabb);
};
/// Encode a floating value into a integer value in order to
/// work with integer comparison in the Sweep-And-Prune algorithm
/// because it is faster. The main issue when encoding floating
/// number into integer is to keep to sorting order. This is a
/// problem for negative float number. This article describes
/// how to solve this issue : http://www.stereopsis.com/radix.html
inline uint encodeFloatIntoInteger(float number) {
uint intNumber = (uint&) number;
// If it's a negative number
if(intNumber & 0x80000000)
intNumber = ~intNumber;
else { // If it is a positive number
intNumber |= 0x80000000;
}
return intNumber;
}
// Check for 1D box intersection between two boxes that are sorted on the given axis.
/// Therefore, only one test is necessary here. We know that the
/// minimum of box1 cannot be larger that the maximum of box2 on the axis.
inline bool SweepAndPruneAlgorithm::testIntersect1DSortedAABBs(const BoxAABB& box1,
const AABBInt& box2,
const EndPoint* const endPointsArray,
uint axis) const {
return !(endPointsArray[box1.max[axis]].value < box2.min[axis]);
}
// Check for 2D box intersection. This method is used when we know
/// that two boxes already overlap on one axis and when want to test
/// if they also overlap on the two others axis.
inline bool SweepAndPruneAlgorithm::testIntersect2D(const BoxAABB& box1, const BoxAABB& box2,
luint axis1, uint axis2) const {
return !(box2.max[axis1] < box1.min[axis1] || box1.max[axis1] < box2.min[axis1] ||
box2.max[axis2] < box1.min[axis2] || box1.max[axis2] < box2.min[axis2]);
}
// Notify the broad-phase that the AABB of an object has changed
inline void SweepAndPruneAlgorithm::updateProxyCollisionShape(CollisionBody* body, const AABB& aabb) {
// Compute the corresponding AABB with integer coordinates
AABBInt aabbInt(aabb);
// Call the update object method that uses an AABB with integer coordinates
updateObjectIntegerAABB(body, aabbInt);
}
}
#endif

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

@ -63,7 +63,7 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const
}
// Constructor
ProxyBoxShape::ProxyBoxShape(const BoxShape* shape, CollisionBody* body,
ProxyBoxShape::ProxyBoxShape(BoxShape* shape, CollisionBody* body,
const Transform& transform, decimal mass)
:ProxyShape(body, transform, mass), mCollisionShape(shape){

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

@ -102,7 +102,7 @@ class BoxShape : public CollisionShape {
/// Create a proxy collision shape for the collision shape
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
const Transform& transform, decimal mass) const;
const Transform& transform, decimal mass);
};
// Class ProxyBoxShape
@ -116,7 +116,7 @@ class ProxyBoxShape : public ProxyShape {
// -------------------- Attributes -------------------- //
/// Pointer to the actual collision shape
const BoxShape* mCollisionShape;
BoxShape* mCollisionShape;
// -------------------- Methods -------------------- //
@ -127,12 +127,15 @@ class ProxyBoxShape : public ProxyShape {
/// Private assignment operator
ProxyBoxShape& operator=(const ProxyBoxShape& proxyShape);
/// Return the non-const collision shape
virtual CollisionShape* getInternalCollisionShape() const;
public:
// -------------------- Methods -------------------- //
/// Constructor
ProxyBoxShape(const BoxShape* shape, CollisionBody* body,
ProxyBoxShape(BoxShape* shape, CollisionBody* body,
const Transform& transform, decimal mass);
/// Destructor
@ -206,11 +209,16 @@ inline bool BoxShape::isEqualTo(const CollisionShape& otherCollisionShape) const
// Create a proxy collision shape for the collision shape
inline ProxyShape* BoxShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
const Transform& transform, decimal mass) const {
const Transform& transform, decimal mass) {
return new (allocator.allocate(sizeof(ProxyBoxShape))) ProxyBoxShape(this, body,
transform, mass);
}
// Return the non-const collision shape
inline CollisionShape* ProxyBoxShape::getInternalCollisionShape() const {
return mCollisionShape;
}
// Return the collision shape
inline const CollisionShape* ProxyBoxShape::getCollisionShape() const {
return mCollisionShape;

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

@ -125,7 +125,7 @@ void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) co
}
// Constructor
ProxyCapsuleShape::ProxyCapsuleShape(const CapsuleShape* shape, CollisionBody* body,
ProxyCapsuleShape::ProxyCapsuleShape(CapsuleShape* shape, CollisionBody* body,
const Transform& transform, decimal mass)
:ProxyShape(body, transform, mass), mCollisionShape(shape){

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

@ -102,7 +102,7 @@ class CapsuleShape : public CollisionShape {
/// Create a proxy collision shape for the collision shape
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
const Transform& transform, decimal mass) const;
const Transform& transform, decimal mass);
};
// Class ProxyCapsuleShape
@ -116,7 +116,7 @@ class ProxyCapsuleShape : public ProxyShape {
// -------------------- Attributes -------------------- //
/// Pointer to the actual collision shape
const CapsuleShape* mCollisionShape;
CapsuleShape* mCollisionShape;
// -------------------- Methods -------------------- //
@ -127,12 +127,15 @@ class ProxyCapsuleShape : public ProxyShape {
/// Private assignment operator
ProxyCapsuleShape& operator=(const ProxyCapsuleShape& proxyShape);
/// Return the non-const collision shape
virtual CollisionShape* getInternalCollisionShape() const;
public:
// -------------------- Methods -------------------- //
/// Constructor
ProxyCapsuleShape(const CapsuleShape* shape, CollisionBody* body,
ProxyCapsuleShape(CapsuleShape* shape, CollisionBody* body,
const Transform& transform, decimal mass);
/// Destructor
@ -197,11 +200,16 @@ inline bool CapsuleShape::isEqualTo(const CollisionShape& otherCollisionShape) c
// Create a proxy collision shape for the collision shape
inline ProxyShape* CapsuleShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
const Transform& transform, decimal mass) const {
const Transform& transform, decimal mass) {
return new (allocator.allocate(sizeof(ProxyCapsuleShape))) ProxyCapsuleShape(this, body,
transform, mass);
}
// Return the non-const collision shape
inline CollisionShape* ProxyCapsuleShape::getInternalCollisionShape() const {
return mCollisionShape;
}
// Return the collision shape
inline const CollisionShape* ProxyCapsuleShape::getCollisionShape() const {
return mCollisionShape;

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

@ -120,7 +120,7 @@ class CollisionShape {
/// Create a proxy collision shape for the collision shape
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
const Transform& transform, decimal mass) const=0;
const Transform& transform, decimal mass)=0;
};
@ -163,6 +163,9 @@ class ProxyShape {
/// Private assignment operator
ProxyShape& operator=(const ProxyShape& proxyShape);
/// Return the non-const collision shape
virtual CollisionShape* getInternalCollisionShape() const=0;
public:
// -------------------- Methods -------------------- //
@ -204,6 +207,7 @@ class ProxyShape {
friend class RigidBody;
friend class BroadPhaseAlgorithm;
friend class DynamicAABBTree;
friend class CollisionDetection;
};
// Return the type of the collision shape

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

@ -94,7 +94,7 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction) c
}
// Constructor
ProxyConeShape::ProxyConeShape(const ConeShape* shape, CollisionBody* body,
ProxyConeShape::ProxyConeShape(ConeShape* shape, CollisionBody* body,
const Transform& transform, decimal mass)
:ProxyShape(body, transform, mass), mCollisionShape(shape){

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

@ -110,7 +110,7 @@ class ConeShape : public CollisionShape {
/// Create a proxy collision shape for the collision shape
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
const Transform& transform, decimal mass) const;
const Transform& transform, decimal mass);
};
// Class ProxyConeShape
@ -124,7 +124,7 @@ class ProxyConeShape : public ProxyShape {
// -------------------- Attributes -------------------- //
/// Pointer to the actual collision shape
const ConeShape* mCollisionShape;
ConeShape* mCollisionShape;
// -------------------- Methods -------------------- //
@ -135,12 +135,15 @@ class ProxyConeShape : public ProxyShape {
/// Private assignment operator
ProxyConeShape& operator=(const ProxyConeShape& proxyShape);
/// Return the non-const collision shape
virtual CollisionShape* getInternalCollisionShape() const;
public:
// -------------------- Methods -------------------- //
/// Constructor
ProxyConeShape(const ConeShape* shape, CollisionBody* body,
ProxyConeShape(ConeShape* shape, CollisionBody* body,
const Transform& transform, decimal mass);
/// Destructor
@ -213,11 +216,16 @@ inline bool ConeShape::isEqualTo(const CollisionShape& otherCollisionShape) cons
// Create a proxy collision shape for the collision shape
inline ProxyShape* ConeShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
const Transform& transform, decimal mass) const {
const Transform& transform, decimal mass) {
return new (allocator.allocate(sizeof(ProxyConeShape))) ProxyConeShape(this, body,
transform, mass);
}
// Return the non-const collision shape
inline CollisionShape* ProxyConeShape::getInternalCollisionShape() const {
return mCollisionShape;
}
// Return the collision shape
inline const CollisionShape* ProxyConeShape::getCollisionShape() const {
return mCollisionShape;

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

@ -225,8 +225,8 @@ bool ConvexMeshShape::isEqualTo(const CollisionShape& otherCollisionShape) const
}
// Constructor
ProxyConvexMeshShape::ProxyConvexMeshShape(const ConvexMeshShape* shape, CollisionBody* body,
const Transform& transform, decimal mass)
ProxyConvexMeshShape::ProxyConvexMeshShape(ConvexMeshShape* shape, CollisionBody* body,
const Transform& transform, decimal mass)
:ProxyShape(body, transform, mass), mCollisionShape(shape),
mCachedSupportVertex(0) {

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

@ -140,7 +140,7 @@ class ConvexMeshShape : public CollisionShape {
/// Create a proxy collision shape for the collision shape
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
const Transform& transform, decimal mass) const;
const Transform& transform, decimal mass);
};
@ -155,7 +155,7 @@ class ProxyConvexMeshShape : public ProxyShape {
// -------------------- Attributes -------------------- //
/// Pointer to the actual collision shape
const ConvexMeshShape* mCollisionShape;
ConvexMeshShape* mCollisionShape;
/// Cached support vertex index (previous support vertex for hill-climbing)
uint mCachedSupportVertex;
@ -168,12 +168,15 @@ class ProxyConvexMeshShape : public ProxyShape {
/// Private assignment operator
ProxyConvexMeshShape& operator=(const ProxyConvexMeshShape& proxyShape);
/// Return the non-const collision shape
virtual CollisionShape* getInternalCollisionShape() const;
public:
// -------------------- Methods -------------------- //
/// Constructor
ProxyConvexMeshShape(const ConvexMeshShape* shape, CollisionBody* body,
ProxyConvexMeshShape(ConvexMeshShape* shape, CollisionBody* body,
const Transform& transform, decimal mass);
/// Destructor
@ -281,11 +284,16 @@ inline void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) {
inline ProxyShape* ConvexMeshShape::createProxyShape(MemoryAllocator& allocator,
CollisionBody* body,
const Transform& transform,
decimal mass) const {
decimal mass) {
return new (allocator.allocate(sizeof(ProxyConvexMeshShape))) ProxyConvexMeshShape(this, body,
transform, mass);
}
// Return the non-const collision shape
inline CollisionShape* ProxyConvexMeshShape::getInternalCollisionShape() const {
return mCollisionShape;
}
// Return the collision shape
inline const CollisionShape* ProxyConvexMeshShape::getCollisionShape() const {
return mCollisionShape;

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

@ -87,7 +87,7 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio
}
// Constructor
ProxyCylinderShape::ProxyCylinderShape(const CylinderShape* cylinderShape, CollisionBody* body,
ProxyCylinderShape::ProxyCylinderShape(CylinderShape* cylinderShape, CollisionBody* body,
const Transform& transform, decimal mass)
:ProxyShape(body, transform, mass), mCollisionShape(cylinderShape){

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

@ -107,7 +107,7 @@ class CylinderShape : public CollisionShape {
/// Create a proxy collision shape for the collision shape
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
const Transform& transform, decimal mass) const;
const Transform& transform, decimal mass);
};
@ -122,8 +122,7 @@ class ProxyCylinderShape : public ProxyShape {
// -------------------- Attributes -------------------- //
/// Pointer to the actual collision shape
const CylinderShape* mCollisionShape;
CylinderShape* mCollisionShape;
// -------------------- Methods -------------------- //
@ -133,12 +132,15 @@ class ProxyCylinderShape : public ProxyShape {
/// Private assignment operator
ProxyCylinderShape& operator=(const ProxyCylinderShape& proxyShape);
/// Return the non-const collision shape
virtual CollisionShape* getInternalCollisionShape() const;
public:
// -------------------- Methods -------------------- //
/// Constructor
ProxyCylinderShape(const CylinderShape* cylinderShape, CollisionBody* body,
ProxyCylinderShape(CylinderShape* cylinderShape, CollisionBody* body,
const Transform& transform, decimal mass);
/// Destructor
@ -211,11 +213,16 @@ inline bool CylinderShape::isEqualTo(const CollisionShape& otherCollisionShape)
// Create a proxy collision shape for the collision shape
inline ProxyShape* CylinderShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
const Transform& transform, decimal mass) const {
const Transform& transform, decimal mass) {
return new (allocator.allocate(sizeof(ProxyCylinderShape))) ProxyCylinderShape(this, body,
transform, mass);
}
// Return the non-const collision shape
inline CollisionShape* ProxyCylinderShape::getInternalCollisionShape() const {
return mCollisionShape;
}
// Return the collision shape
inline const CollisionShape* ProxyCylinderShape::getCollisionShape() const {
return mCollisionShape;

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

@ -47,7 +47,7 @@ SphereShape::~SphereShape() {
}
// Constructor
ProxySphereShape::ProxySphereShape(const SphereShape* shape, CollisionBody* body,
ProxySphereShape::ProxySphereShape(SphereShape* shape, CollisionBody* body,
const Transform& transform, decimal mass)
:ProxyShape(body, transform, mass), mCollisionShape(shape){

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

@ -97,7 +97,7 @@ class SphereShape : public CollisionShape {
/// Create a proxy collision shape for the collision shape
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
const Transform& transform, decimal mass) const;
const Transform& transform, decimal mass);
};
@ -112,7 +112,7 @@ class ProxySphereShape : public ProxyShape {
// -------------------- Attributes -------------------- //
/// Pointer to the actual collision shape
const SphereShape* mCollisionShape;
SphereShape* mCollisionShape;
// -------------------- Methods -------------------- //
@ -123,12 +123,15 @@ class ProxySphereShape : public ProxyShape {
/// Private assignment operator
ProxySphereShape& operator=(const ProxySphereShape& proxyShape);
/// Return the non-const collision shape
virtual CollisionShape* getInternalCollisionShape() const;
public:
// -------------------- Methods -------------------- //
/// Constructor
ProxySphereShape(const SphereShape* shape, CollisionBody* body,
ProxySphereShape(SphereShape* shape, CollisionBody* body,
const Transform& transform, decimal mass);
/// Destructor
@ -229,11 +232,16 @@ inline bool SphereShape::isEqualTo(const CollisionShape& otherCollisionShape) co
// Create a proxy collision shape for the collision shape
inline ProxyShape* SphereShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
const Transform& transform, decimal mass) const {
const Transform& transform, decimal mass) {
return new (allocator.allocate(sizeof(ProxySphereShape))) ProxySphereShape(this, body,
transform, mass);
}
// Return the non-const collision shape
inline CollisionShape* ProxySphereShape::getInternalCollisionShape() const {
return mCollisionShape;
}
// Return the collision shape
inline const CollisionShape* ProxySphereShape::getCollisionShape() const {
return mCollisionShape;

2
src/constraint/SliderJoint.cpp
View File

@ -76,7 +76,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
// Get the bodies positions and orientations
const Vector3& x1 = mBody1-mCenterOfMassWorld;
const Vector3& x1 = mBody1->mCenterOfMassWorld;
const Vector3& x2 = mBody2->mCenterOfMassWorld;
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();

27
src/engine/CollisionWorld.cpp
View File

@ -33,7 +33,8 @@ using namespace std;
// Constructor
CollisionWorld::CollisionWorld()
: mCollisionDetection(this, mMemoryAllocator), mCurrentBodyID(0) {
: mCollisionDetection(this, mMemoryAllocator), mCurrentBodyID(0),
mEventListener(NULL) {
}
@ -43,21 +44,8 @@ CollisionWorld::~CollisionWorld() {
assert(mBodies.empty());
}
// Notify the world about a new narrow-phase contact
void CollisionWorld::notifyNewContact(const OverlappingPair *broadPhasePair,
const ContactPointInfo* contactInfo) {
// TODO : Implement this method
}
// Update the overlapping pair
inline void CollisionWorld::updateOverlappingPair(const OverlappingPair *pair) {
}
// Create a collision body and add it to the world
CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform,
CollisionShape* collisionShape) {
CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) {
// Get the next available body ID
bodyindex bodyID = computeNextAvailableBodyID();
@ -67,16 +55,13 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform,
// Create the collision body
CollisionBody* collisionBody = new (mMemoryAllocator.allocate(sizeof(CollisionBody)))
CollisionBody(transform, collisionShape, bodyID);
CollisionBody(transform, *this, bodyID);
assert(collisionBody != NULL);
// Add the collision body to the world
mBodies.insert(collisionBody);
// Add the collision body to the collision detection
mCollisionDetection.addProxyCollisionShape(collisionBody);
// Return the pointer to the rigid body
return collisionBody;
}
@ -84,8 +69,8 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform,
// Destroy a collision body
void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
// Remove the body from the collision detection
mCollisionDetection.removeProxyCollisionShape(collisionBody);
// Remove all the collision shapes of the body
collisionBody->removeAllCollisionShapes();
// Add the body ID to the list of free IDs
mFreeBodiesIDs.push_back(collisionBody->getID());

13
src/engine/CollisionWorld.h
View File

@ -77,6 +77,9 @@ class CollisionWorld {
/// Memory allocator
MemoryAllocator mMemoryAllocator;
/// Pointer to an event listener object
EventListener* mEventListener;
// -------------------- Methods -------------------- //
/// Private copy-constructor
@ -85,13 +88,6 @@ class CollisionWorld {
/// Private assignment operator
CollisionWorld& operator=(const CollisionWorld& world);
/// Notify the world about a new narrow-phase contact
virtual void notifyNewContact(const OverlappingPair* pair,
const ContactPointInfo* contactInfo);
/// Update the overlapping pair
virtual void updateOverlappingPair(const OverlappingPair* pair);
/// Return the next available body ID
bodyindex computeNextAvailableBodyID();
@ -118,8 +114,7 @@ class CollisionWorld {
std::set<CollisionBody*>::iterator getBodiesEndIterator();
/// Create a collision body
CollisionBody* createCollisionBody(const Transform& transform,
CollisionShape* collisionShape);
CollisionBody* createCollisionBody(const Transform& transform);
/// Destroy a collision body
void destroyCollisionBody(CollisionBody* collisionBody);

13
src/engine/ConstraintSolver.cpp
View File

@ -30,14 +30,9 @@
using namespace reactphysics3d;
// Constructor
ConstraintSolver::ConstraintSolver(std::vector<Vector3>& positions,
std::vector<Quaternion>& orientations,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
: mLinearVelocities(NULL), mAngularVelocities(NULL), mPositions(positions),
mOrientations(orientations),
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true), mConstraintSolverData(positions, orientations,
mapBodyToVelocityIndex){
ConstraintSolver::ConstraintSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
: mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
mIsWarmStartingActive(true), mConstraintSolverData(mapBodyToVelocityIndex) {
}
@ -51,8 +46,6 @@ void ConstraintSolver::initializeForIsland(decimal dt, Island* island) {
PROFILE("ConstraintSolver::initializeForIsland()");
assert(mLinearVelocities != NULL);
assert(mAngularVelocities != NULL);
assert(island != NULL);
assert(island->getNbBodies() > 0);
assert(island->getNbJoints() > 0);

49
src/engine/ConstraintSolver.h
View File

@ -55,10 +55,10 @@ struct ConstraintSolverData {
Vector3* angularVelocities;
/// Reference to the bodies positions
std::vector<Vector3>& positions;
Vector3* positions;
/// Reference to the bodies orientations
std::vector<Quaternion>& orientations;
Quaternion* orientations;
/// Reference to the map that associates rigid body to their index
/// in the constrained velocities array
@ -68,12 +68,9 @@ struct ConstraintSolverData {
bool isWarmStartingActive;
/// Constructor
ConstraintSolverData(std::vector<Vector3>& refPositions,
std::vector<Quaternion>& refOrientations,
const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex)
:linearVelocities(NULL),
angularVelocities(NULL),
positions(refPositions), orientations(refOrientations),
ConstraintSolverData(const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex)
:linearVelocities(NULL), angularVelocities(NULL),
positions(NULL), orientations(NULL),
mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){
}
@ -155,20 +152,6 @@ class ConstraintSolver {
// -------------------- Attributes -------------------- //
/// Array of constrained linear velocities (state of the linear velocities
/// after solving the constraints)
Vector3* mLinearVelocities;
/// Array of constrained angular velocities (state of the angular velocities
/// after solving the constraints)
Vector3* mAngularVelocities;
/// Reference to the array of bodies positions (for position error correction)
std::vector<Vector3>& mPositions;
/// Reference to the array of bodies orientations (for position error correction)
std::vector<Quaternion>& mOrientations;
/// Reference to the map that associates rigid body to their index in
/// the constrained velocities array
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
@ -187,8 +170,7 @@ class ConstraintSolver {
// -------------------- Methods -------------------- //
/// Constructor
ConstraintSolver(std::vector<Vector3>& positions, std::vector<Quaternion>& orientations,
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
ConstraintSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
/// Destructor
~ConstraintSolver();
@ -211,6 +193,10 @@ class ConstraintSolver {
/// Set the constrained velocities arrays
void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
Vector3* constrainedAngularVelocities);
/// Set the constrained positions/orientations arrays
void setConstrainedPositionsArrays(Vector3* constrainedPositions,
Quaternion* constrainedOrientations);
};
// Set the constrained velocities arrays
@ -218,10 +204,17 @@ inline void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3* constraine
Vector3* constrainedAngularVelocities) {
assert(constrainedLinearVelocities != NULL);
assert(constrainedAngularVelocities != NULL);
mLinearVelocities = constrainedLinearVelocities;
mAngularVelocities = constrainedAngularVelocities;
mConstraintSolverData.linearVelocities = mLinearVelocities;
mConstraintSolverData.angularVelocities = mAngularVelocities;
mConstraintSolverData.linearVelocities = constrainedLinearVelocities;
mConstraintSolverData.angularVelocities = constrainedAngularVelocities;
}
// Set the constrained positions/orientations arrays
inline void ConstraintSolver::setConstrainedPositionsArrays(Vector3* constrainedPositions,
Quaternion* constrainedOrientations) {
assert(constrainedPositions != NULL);
assert(constrainedOrientations != NULL);
mConstraintSolverData.positions = constrainedPositions;
mConstraintSolverData.orientations = constrainedOrientations;
}
}

183
src/engine/DynamicsWorld.cpp
View File

@ -46,9 +46,9 @@ using namespace std;
DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
: CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityEnabled(true),
mConstrainedLinearVelocities(NULL), mConstrainedAngularVelocities(NULL),
mConstrainedPositions(NULL), mConstrainedOrientations(NULL),
mContactSolver(mMapBodyToConstrainedVelocityIndex),
mConstraintSolver(mConstrainedPositions, mConstrainedOrientations,
mMapBodyToConstrainedVelocityIndex),
mConstraintSolver(mMapBodyToConstrainedVelocityIndex),
mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
mIsSleepingEnabled(SPLEEPING_ENABLED), mSplitLinearVelocities(NULL),
@ -56,7 +56,7 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_
mIslands(NULL), mNbBodiesCapacity(0),
mSleepLinearVelocity(DEFAULT_SLEEP_LINEAR_VELOCITY),
mSleepAngularVelocity(DEFAULT_SLEEP_ANGULAR_VELOCITY),
mTimeBeforeSleep(DEFAULT_TIME_BEFORE_SLEEP), mEventListener(NULL) {
mTimeBeforeSleep(DEFAULT_TIME_BEFORE_SLEEP) {
}
@ -90,6 +90,8 @@ DynamicsWorld::~DynamicsWorld() {
delete[] mSplitAngularVelocities;
delete[] mConstrainedLinearVelocities;
delete[] mConstrainedAngularVelocities;
delete[] mConstrainedPositions;
delete[] mConstrainedOrientations;
}
#ifdef IS_PROFILING_ACTIVE
@ -122,7 +124,7 @@ void DynamicsWorld::update() {
while(mTimer.isPossibleToTakeStep()) {
// Remove all contact manifolds
mContactManifolds.clear();
mCollisionDetection.mContactManifolds.clear();
// Reset all the contact manifolds lists of each body
resetContactManifoldListsOfBodies();
@ -148,10 +150,10 @@ void DynamicsWorld::update() {
// Solve the position correction for constraints
solvePositionCorrection();
if (mIsSleepingEnabled) updateSleepingBodies();
// Update the state (positions and velocities) of the bodies
updateBodiesState();
// Update the AABBs of the bodies
updateRigidBodiesAABB();
if (mIsSleepingEnabled) updateSleepingBodies();
}
// Reset the external force and torque applied to the bodies
@ -183,10 +185,6 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
Vector3 newAngVelocity = mConstrainedAngularVelocities[indexArray];
// Update the linear and angular velocity of the body
bodies[b]->mLinearVelocity = newLinVelocity;
bodies[b]->mAngularVelocity = newAngVelocity;
// Add the split impulse velocity from Contact Solver (only used
// to update the position)
if (mContactSolver.isSplitImpulseActive()) {
@ -199,38 +197,45 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
const Vector3& currentPosition = bodies[b]->getTransform().getPosition();
const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation();
// Compute the new position of the body
Vector3 newPosition = currentPosition + newLinVelocity * dt;
Quaternion newOrientation = currentOrientation + Quaternion(0, newAngVelocity) *
currentOrientation * decimal(0.5) * dt;
// Update the world-space center of mass
// TODO : IMPLEMENT THIS
// Update the Transform of the body
Transform newTransform(newPosition, newOrientation.getUnit());
bodies[b]->setTransform(newTransform);
// Update the new constrained position and orientation of the body
mConstrainedPositions[indexArray] = currentPosition + newLinVelocity * dt;
mConstrainedOrientations[indexArray] = currentOrientation +
Quaternion(0, newAngVelocity) *
currentOrientation * decimal(0.5) * dt;
}
}
}
// Update the AABBs of the bodies
void DynamicsWorld::updateRigidBodiesAABB() {
// Update the postion/orientation of the bodies
void DynamicsWorld::updateBodiesState() {
PROFILE("DynamicsWorld::updateRigidBodiesAABB()");
PROFILE("DynamicsWorld::updateBodiesState()");
// For each rigid body of the world
set<RigidBody*>::iterator it;
for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
// For each island of the world
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
// If the body has moved
if ((*it)->mHasMoved) {
// For each body of the island
RigidBody** bodies = mIslands[islandIndex]->getBodies();
// Update the transform of the body due to the change of its center of mass
(*it)->updateTransformWithCenterOfMass();
for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
// Update the AABB of the rigid body
(*it)->updateAABB();
uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
// Update the linear and angular velocity of the body
bodies[b]->mLinearVelocity = mConstrainedLinearVelocities[index];
bodies[b]->mAngularVelocity = mConstrainedAngularVelocities[index];
// Update the position of the center of mass of the body
bodies[b]->mCenterOfMassWorld = mConstrainedPositions[index];
// Update the orientation of the body
bodies[b]->mTransform.setOrientation(mConstrainedOrientations[index].getUnit());
// Update the transform of the body (using the new center of mass and new orientation)
bodies[b]->updateTransformWithCenterOfMass();
// Update the broad-phase state of the body
bodies[b]->updateBroadPhaseState();
}
}
}
@ -263,14 +268,19 @@ void DynamicsWorld::initVelocityArrays() {
delete[] mSplitAngularVelocities;
}
mNbBodiesCapacity = nbBodies;
// TODO : Use better memory allocation here
mSplitLinearVelocities = new Vector3[mNbBodiesCapacity];
mSplitAngularVelocities = new Vector3[mNbBodiesCapacity];
mConstrainedLinearVelocities = new Vector3[mNbBodiesCapacity];
mConstrainedAngularVelocities = new Vector3[mNbBodiesCapacity];
mConstrainedPositions = new Vector3[mNbBodiesCapacity];
mConstrainedOrientations = new Quaternion[mNbBodiesCapacity];
assert(mSplitLinearVelocities != NULL);
assert(mSplitAngularVelocities != NULL);
assert(mConstrainedLinearVelocities != NULL);
assert(mConstrainedAngularVelocities != NULL);
assert(mConstrainedPositions != NULL);
assert(mConstrainedOrientations != NULL);
}
// Reset the velocities arrays
@ -381,6 +391,8 @@ void DynamicsWorld::solveContactsAndConstraints() {
mConstrainedAngularVelocities);
mConstraintSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
mConstrainedAngularVelocities);
mConstraintSolver.setConstrainedPositionsArrays(mConstrainedPositions,
mConstrainedOrientations);
// ---------- Solve velocity constraints for joints and contacts ---------- //
@ -440,10 +452,6 @@ void DynamicsWorld::solvePositionCorrection() {
// ---------- Get the position/orientation of the rigid bodies ---------- //
// TODO : Use better memory allocation here
mConstrainedPositions = std::vector<Vector3>(mRigidBodies.size());
mConstrainedOrientations = std::vector<Quaternion>(mRigidBodies.size());
// For each island of the world
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
@ -467,27 +475,11 @@ void DynamicsWorld::solvePositionCorrection() {
// Solve the position constraints
mConstraintSolver.solvePositionConstraints(mIslands[islandIndex]);
}
// ---------- Update the position/orientation of the rigid bodies ---------- //
for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
// Update the position of the center of mass of the body
bodies[b]->mCenterOfMassWorld = mConstrainedPositions[index];
// Update the orientation of the body
bodies[b]->mTransform.setOrientation(mConstrainedOrientations[index].getUnit());
// Update the Transform of the body (using the new center of mass and new orientation)
bodies[b]->updateTransformWithCenterOfMass();
}
}
}
// Create a rigid body into the physics world
RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal mass) {
RigidBody* DynamicsWorld::createRigidBody(const Transform& transform) {
// Compute the body ID
bodyindex bodyID = computeNextAvailableBodyID();
@ -497,17 +489,13 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal ma
// Create the rigid body
RigidBody* rigidBody = new (mMemoryAllocator.allocate(sizeof(RigidBody))) RigidBody(transform,
mass, bodyID);
*this, bodyID);
assert(rigidBody != NULL);
// Add the rigid body to the physics world
mBodies.insert(rigidBody);
mRigidBodies.insert(rigidBody);
// TODO : DELETE THIS
// Add the rigid body to the collision detection
//mCollisionDetection.addProxyCollisionShape(rigidBody);
// Return the pointer to the rigid body
return rigidBody;
}
@ -515,17 +503,12 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal ma
// Destroy a rigid body and all the joints which it belongs
void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
// TODO : DELETE THIS
// Remove the body from the collision detection
//mCollisionDetection.removeProxyCollisionShape(rigidBody);
// Remove all the collision shapes of the body
rigidBody->removeAllCollisionShapes();
// Add the body ID to the list of free IDs
mFreeBodiesIDs.push_back(rigidBody->getID());
// TODO : DELETE THIS
// Remove the collision shape from the world
//removeCollisionShape(rigidBody->getCollisionShape());
// Destroy all the joints in which the rigid body to be destroyed is involved
JointListElement* element;
for (element = rigidBody->mJointsList; element != NULL; element = element->next) {
@ -665,29 +648,6 @@ void DynamicsWorld::addJointToBody(Joint* joint) {
joint->mBody2->mJointsList = jointListElement2;
}
// Add a contact manifold to the linked list of contact manifolds of the two bodies involed
// in the corresponding contact
void DynamicsWorld::addContactManifoldToBody(ContactManifold* contactManifold,
CollisionBody* body1, CollisionBody* body2) {
assert(contactManifold != NULL);
// Add the contact manifold at the beginning of the linked
// list of contact manifolds of the first body
void* allocatedMemory1 = mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement1 = new (allocatedMemory1)
ContactManifoldListElement(contactManifold,
body1->mContactManifoldsList);
body1->mContactManifoldsList = listElement1;
// Add the joint at the beginning of the linked list of joints of the second body
void* allocatedMemory2 = mMemoryAllocator.allocate(sizeof(ContactManifoldListElement));
ContactManifoldListElement* listElement2 = new (allocatedMemory2)
ContactManifoldListElement(contactManifold,
body2->mContactManifoldsList);
body2->mContactManifoldsList = listElement2;
}
// Reset all the contact manifolds linked list of each body
void DynamicsWorld::resetContactManifoldListsOfBodies() {
@ -736,8 +696,8 @@ void DynamicsWorld::computeIslands() {
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
(*it)->mIsAlreadyInIsland = false;
}
for (std::vector<ContactManifold*>::iterator it = mContactManifolds.begin();
it != mContactManifolds.end(); ++it) {
for (std::vector<ContactManifold*>::iterator it = mCollisionDetection.mContactManifolds.begin();
it != mCollisionDetection.mContactManifolds.end(); ++it) {
(*it)->mIsAlreadyInIsland = false;
}
for (std::set<Joint*>::iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
@ -770,7 +730,8 @@ void DynamicsWorld::computeIslands() {
// Create the new island
void* allocatedMemoryIsland = mMemoryAllocator.allocate(sizeof(Island));
mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies,mContactManifolds.size(),
mIslands[mNbIslands] = new (allocatedMemoryIsland) Island(nbBodies,
mCollisionDetection.mContactManifolds.size(),
mJoints.size(), mMemoryAllocator);
// While there are still some bodies to visit in the stack
@ -919,42 +880,6 @@ void DynamicsWorld::updateSleepingBodies() {
}
}
// Notify the world about a new narrow-phase contact
void DynamicsWorld::notifyNewContact(const BroadPhasePair* broadPhasePair,
const ContactPointInfo* contactInfo) {
// Create a new contact
ContactPoint* contact = new (mMemoryAllocator.allocate(sizeof(ContactPoint))) ContactPoint(
*contactInfo);
assert(contact != NULL);
// Get the corresponding overlapping pair
pair<bodyindex, bodyindex> indexPair = broadPhasePair->getBodiesIndexPair();
OverlappingPair* overlappingPair = mOverlappingPairs.find(indexPair)->second;
assert(overlappingPair != NULL);
// If it is the first contact since the pair are overlapping
if (overlappingPair->getNbContactPoints() == 0) {
// Trigger a callback event
if (mEventListener != NULL) mEventListener->beginContact(*contactInfo);
}
// Add the contact to the contact cache of the corresponding overlapping pair
overlappingPair->addContact(contact);
// Add the contact manifold to the world
mContactManifolds.push_back(overlappingPair->getContactManifold());
// Add the contact manifold into the list of contact manifolds
// of the two bodies involved in the contact
addContactManifoldToBody(overlappingPair->getContactManifold(), overlappingPair->mBody1,
overlappingPair->mBody2);
// Trigger a callback event for the new contact
if (mEventListener != NULL) mEventListener->newContact(*contactInfo);
}
// Enable/Disable the sleeping technique
void DynamicsWorld::enableSleeping(bool isSleepingEnabled) {
mIsSleepingEnabled = isSleepingEnabled;

49
src/engine/DynamicsWorld.h
View File

@ -72,10 +72,6 @@ class DynamicsWorld : public CollisionWorld {
/// All the rigid bodies of the physics world
std::set<RigidBody*> mRigidBodies;
/// All the contact constraints
// TODO : Remove this variable (we will use the ones in the island now)
std::vector<ContactManifold*> mContactManifolds;
/// All the joints of the world
std::set<Joint*> mJoints;
@ -100,10 +96,10 @@ class DynamicsWorld : public CollisionWorld {
Vector3* mSplitAngularVelocities;
/// Array of constrained rigid bodies position (for position error correction)
std::vector<Vector3> mConstrainedPositions;
Vector3* mConstrainedPositions;
/// Array of constrained rigid bodies orientation (for position error correction)
std::vector<Quaternion> mConstrainedOrientations;
Quaternion* mConstrainedOrientations;
/// Map body to their index in the constrained velocities array
std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex;
@ -130,9 +126,6 @@ class DynamicsWorld : public CollisionWorld {
/// becomes smaller than the sleep velocity.
decimal mTimeBeforeSleep;
/// Pointer to an event listener object
EventListener* mEventListener;
// -------------------- Methods -------------------- //
/// Private copy-constructor
@ -154,11 +147,6 @@ class DynamicsWorld : public CollisionWorld {
void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
Vector3 newAngVelocity);
/// 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);
/// Compute and set the interpolation factor to all bodies
void setInterpolationFactorToAllBodies();
@ -180,16 +168,12 @@ class DynamicsWorld : public CollisionWorld {
/// Compute the islands of awake bodies.
void computeIslands();
/// Update the postion/orientation of the bodies
void updateBodiesState();
/// Put bodies to sleep if needed.
void updateSleepingBodies();
/// Update the overlapping pair
virtual void updateOverlappingPair(const BroadPhasePair* pair);
/// Notify the world about a new narrow-phase contact
virtual void notifyNewContact(const BroadPhasePair* pair,
const ContactPointInfo* contactInfo);
public :
// -------------------- Methods -------------------- //
@ -226,7 +210,7 @@ class DynamicsWorld : public CollisionWorld {
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
/// Create a rigid body into the physics world.
RigidBody* createRigidBody(const Transform& transform, decimal mass);
RigidBody* createRigidBody(const Transform& transform);
/// Destroy a rigid body and all the joints which it belongs
void destroyRigidBody(RigidBody* rigidBody);
@ -359,21 +343,6 @@ inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool
mContactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive);
}
// Update the overlapping pair
inline void DynamicsWorld::updateOverlappingPair(const BroadPhasePair* pair) {
// TODO : CHECK WHERE TO CALL THIS METHOD
// Get the pair of body index
std::pair<bodyindex, bodyindex> indexPair = pair->getBodiesIndexPair();
// Get the corresponding overlapping pair
OverlappingPair* overlappingPair = mOverlappingPairs[indexPair];
// Update the contact cache of the overlapping pair
overlappingPair->update();
}
// Return the gravity vector of the world
inline Vector3 DynamicsWorld::getGravity() const {
return mGravity;
@ -410,13 +379,15 @@ inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesEndIterator()
}
// Return a reference to the contact manifolds of the world
// TODO : DELETE THIS METHOD AND USE EVENT LISTENER IN EXAMPLES INSTEAD
inline const std::vector<ContactManifold*>& DynamicsWorld::getContactManifolds() const {
return mContactManifolds;
return mCollisionDetection.mContactManifolds;
}
// Return the number of contact manifolds in the world
// TODO : DELETE THIS METHOD AND USE EVENT LISTENER IN EXAMPLES INSTEAD
inline uint DynamicsWorld::getNbContactManifolds() const {
return mContactManifolds.size();
return mCollisionDetection.mContactManifolds.size();
}
// Return the current physics time (in seconds)

7
src/engine/OverlappingPair.cpp
View File

@ -30,9 +30,10 @@ using namespace reactphysics3d;
// Constructor
OverlappingPair::OverlappingPair(CollisionBody* body1, CollisionBody* body2,
MemoryAllocator &memoryAllocator)
: mBody1(body1), mBody2(body2), mContactManifold(body1, body2, memoryAllocator),
OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
MemoryAllocator& memoryAllocator)
: mShape1(shape1), mShape2(shape2),
mContactManifold(shape1->getBody(), shape2->getBody(), memoryAllocator),
mCachedSeparatingAxis(1.0, 1.0, 1.0) {
}

3
src/engine/OverlappingPair.h
View File

@ -133,7 +133,8 @@ inline void OverlappingPair::addContact(ContactPoint* contact) {
// Update the contact manifold
inline void OverlappingPair::update() {
mContactManifold.update(mShape1->getBody()->getTransform(), mShape2->getBody()->getTransform());
mContactManifold.update(mShape1->getBody()->getTransform() *mShape1->getLocalToBodyTransform(),
mShape2->getBody()->getTransform() *mShape2->getLocalToBodyTransform());
}
// Return the cached separating axis