From aa76c85e60a822876cb059c3595d7e18b2e336e4 Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Thu, 15 May 2014 06:39:39 +0200
Subject: [PATCH] continue to replace SAP broad-phase by a dynamic AABB tree
---
CMakeLists.txt | 4 -
examples/common/Box.cpp | 7 +-
examples/common/Capsule.cpp | 7 +-
examples/common/Cone.cpp | 5 +-
examples/common/ConvexMesh.cpp | 5 +-
examples/common/Cylinder.cpp | 5 +-
examples/common/Sphere.cpp | 5 +-
src/body/CollisionBody.cpp | 11 +-
src/body/CollisionBody.h | 5 +-
src/body/RigidBody.cpp | 21 +-
src/body/RigidBody.h | 8 +-
src/collision/CollisionDetection.cpp | 96 ++-
src/collision/CollisionDetection.h | 18 +-
.../broadphase/BroadPhaseAlgorithm.cpp | 3 +-
.../broadphase/BroadPhaseAlgorithm.h | 22 -
.../broadphase/NoBroadPhaseAlgorithm.cpp | 43 --
.../broadphase/NoBroadPhaseAlgorithm.h | 123 ----
.../broadphase/SweepAndPruneAlgorithm.cpp | 596 ------------------
.../broadphase/SweepAndPruneAlgorithm.h | 245 -------
src/collision/shapes/BoxShape.cpp | 2 +-
src/collision/shapes/BoxShape.h | 16 +-
src/collision/shapes/CapsuleShape.cpp | 2 +-
src/collision/shapes/CapsuleShape.h | 16 +-
src/collision/shapes/CollisionShape.h | 6 +-
src/collision/shapes/ConeShape.cpp | 2 +-
src/collision/shapes/ConeShape.h | 16 +-
src/collision/shapes/ConvexMeshShape.cpp | 4 +-
src/collision/shapes/ConvexMeshShape.h | 16 +-
src/collision/shapes/CylinderShape.cpp | 2 +-
src/collision/shapes/CylinderShape.h | 17 +-
src/collision/shapes/SphereShape.cpp | 2 +-
src/collision/shapes/SphereShape.h | 16 +-
src/constraint/SliderJoint.cpp | 2 +-
src/engine/CollisionWorld.cpp | 27 +-
src/engine/CollisionWorld.h | 13 +-
src/engine/ConstraintSolver.cpp | 13 +-
src/engine/ConstraintSolver.h | 49 +-
src/engine/DynamicsWorld.cpp | 183 ++----
src/engine/DynamicsWorld.h | 49 +-
src/engine/OverlappingPair.cpp | 7 +-
src/engine/OverlappingPair.h | 3 +-
41 files changed, 322 insertions(+), 1370 deletions(-)
delete mode 100644 src/collision/broadphase/NoBroadPhaseAlgorithm.cpp
delete mode 100644 src/collision/broadphase/NoBroadPhaseAlgorithm.h
delete mode 100644 src/collision/broadphase/SweepAndPruneAlgorithm.cpp
delete mode 100644 src/collision/broadphase/SweepAndPruneAlgorithm.h
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0f3289c1..f2518de6 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -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"
diff --git a/examples/common/Box.cpp b/examples/common/Box.cpp
index c619937c..817080b2 100644
--- a/examples/common/Box.cpp
+++ b/examples/common/Box.cpp
@@ -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) {
diff --git a/examples/common/Capsule.cpp b/examples/common/Capsule.cpp
index ea0913e5..d593281f 100644
--- a/examples/common/Capsule.cpp
+++ b/examples/common/Capsule.cpp
@@ -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
diff --git a/examples/common/Cone.cpp b/examples/common/Cone.cpp
index 736c06fe..df6714c7 100644
--- a/examples/common/Cone.cpp
+++ b/examples/common/Cone.cpp
@@ -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
diff --git a/examples/common/ConvexMesh.cpp b/examples/common/ConvexMesh.cpp
index c2fc7369..725c68e5 100644
--- a/examples/common/ConvexMesh.cpp
+++ b/examples/common/ConvexMesh.cpp
@@ -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
diff --git a/examples/common/Cylinder.cpp b/examples/common/Cylinder.cpp
index cfbdfec6..15f4756c 100644
--- a/examples/common/Cylinder.cpp
+++ b/examples/common/Cylinder.cpp
@@ -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
diff --git a/examples/common/Sphere.cpp b/examples/common/Sphere.cpp
index f9fbb1d0..d8af5af7 100644
--- a/examples/common/Sphere.cpp
+++ b/examples/common/Sphere.cpp
@@ -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
diff --git a/src/body/CollisionBody.cpp b/src/body/CollisionBody.cpp
index 057c95cd..ab4c1b34 100644
--- a/src/body/CollisionBody.cpp
+++ b/src/body/CollisionBody.cpp
@@ -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));
diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h
index 86fe32e3..e78bf04b 100644
--- a/src/body/CollisionBody.h
+++ b/src/body/CollisionBody.h
@@ -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;
diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp
index b36f3d83..ce0e271b 100644
--- a/src/body/RigidBody.cpp
+++ b/src/body/RigidBody.cpp
@@ -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();
diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h
index 075c0640..16913a8a 100644
--- a/src/body/RigidBody.h
+++ b/src/body/RigidBody.h
@@ -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.
diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp
index 06243de4..47bfc913 100644
--- a/src/collision/CollisionDetection.cpp
+++ b/src/collision/CollisionDetection.cpp
@@ -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;
+}
diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h
index ed209b6f..790b2286 100644
--- a/src/collision/CollisionDetection.h
+++ b/src/collision/CollisionDetection.h
@@ -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
diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.cpp b/src/collision/broadphase/BroadPhaseAlgorithm.cpp
index 148da8a3..2b554ae9 100644
--- a/src/collision/broadphase/BroadPhaseAlgorithm.cpp
+++ b/src/collision/broadphase/BroadPhaseAlgorithm.cpp
@@ -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));
diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.h b/src/collision/broadphase/BroadPhaseAlgorithm.h
index 0cc883b8..443e4d62 100644
--- a/src/collision/broadphase/BroadPhaseAlgorithm.h
+++ b/src/collision/broadphase/BroadPhaseAlgorithm.h
@@ -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
diff --git a/src/collision/broadphase/NoBroadPhaseAlgorithm.cpp b/src/collision/broadphase/NoBroadPhaseAlgorithm.cpp
deleted file mode 100644
index d0d3af5d..00000000
--- a/src/collision/broadphase/NoBroadPhaseAlgorithm.cpp
+++ /dev/null
@@ -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() {
-
-}
diff --git a/src/collision/broadphase/NoBroadPhaseAlgorithm.h b/src/collision/broadphase/NoBroadPhaseAlgorithm.h
deleted file mode 100644
index d153b057..00000000
--- a/src/collision/broadphase/NoBroadPhaseAlgorithm.h
+++ /dev/null
@@ -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
-
-
diff --git a/src/collision/broadphase/SweepAndPruneAlgorithm.cpp b/src/collision/broadphase/SweepAndPruneAlgorithm.cpp
deleted file mode 100644
index 06691573..00000000
--- a/src/collision/broadphase/SweepAndPruneAlgorithm.cpp
+++ /dev/null
@@ -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;
-}
diff --git a/src/collision/broadphase/SweepAndPruneAlgorithm.h b/src/collision/broadphase/SweepAndPruneAlgorithm.h
deleted file mode 100644
index 0c0225c4..00000000
--- a/src/collision/broadphase/SweepAndPruneAlgorithm.h
+++ /dev/null
@@ -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
-
-
diff --git a/src/collision/shapes/BoxShape.cpp b/src/collision/shapes/BoxShape.cpp
index e3afb84c..5e838724 100644
--- a/src/collision/shapes/BoxShape.cpp
+++ b/src/collision/shapes/BoxShape.cpp
@@ -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){
diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h
index f4a6e7ae..0e4e4279 100644
--- a/src/collision/shapes/BoxShape.h
+++ b/src/collision/shapes/BoxShape.h
@@ -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;
diff --git a/src/collision/shapes/CapsuleShape.cpp b/src/collision/shapes/CapsuleShape.cpp
index 97f63828..eb4d6815 100644
--- a/src/collision/shapes/CapsuleShape.cpp
+++ b/src/collision/shapes/CapsuleShape.cpp
@@ -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){
diff --git a/src/collision/shapes/CapsuleShape.h b/src/collision/shapes/CapsuleShape.h
index bf532369..16214104 100644
--- a/src/collision/shapes/CapsuleShape.h
+++ b/src/collision/shapes/CapsuleShape.h
@@ -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;
diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h
index 0623febe..3a08170f 100644
--- a/src/collision/shapes/CollisionShape.h
+++ b/src/collision/shapes/CollisionShape.h
@@ -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
diff --git a/src/collision/shapes/ConeShape.cpp b/src/collision/shapes/ConeShape.cpp
index ef2b958f..3ea602c4 100644
--- a/src/collision/shapes/ConeShape.cpp
+++ b/src/collision/shapes/ConeShape.cpp
@@ -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){
diff --git a/src/collision/shapes/ConeShape.h b/src/collision/shapes/ConeShape.h
index a6232127..f9f632b8 100644
--- a/src/collision/shapes/ConeShape.h
+++ b/src/collision/shapes/ConeShape.h
@@ -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;
diff --git a/src/collision/shapes/ConvexMeshShape.cpp b/src/collision/shapes/ConvexMeshShape.cpp
index a59c2b37..943b2fc6 100644
--- a/src/collision/shapes/ConvexMeshShape.cpp
+++ b/src/collision/shapes/ConvexMeshShape.cpp
@@ -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) {
diff --git a/src/collision/shapes/ConvexMeshShape.h b/src/collision/shapes/ConvexMeshShape.h
index 8d4a43e9..220afa3f 100644
--- a/src/collision/shapes/ConvexMeshShape.h
+++ b/src/collision/shapes/ConvexMeshShape.h
@@ -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;
diff --git a/src/collision/shapes/CylinderShape.cpp b/src/collision/shapes/CylinderShape.cpp
index 13f62cac..5691d5e7 100644
--- a/src/collision/shapes/CylinderShape.cpp
+++ b/src/collision/shapes/CylinderShape.cpp
@@ -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){
diff --git a/src/collision/shapes/CylinderShape.h b/src/collision/shapes/CylinderShape.h
index 4e697501..a290a51d 100644
--- a/src/collision/shapes/CylinderShape.h
+++ b/src/collision/shapes/CylinderShape.h
@@ -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;
diff --git a/src/collision/shapes/SphereShape.cpp b/src/collision/shapes/SphereShape.cpp
index f1cd1e0d..777b4bb4 100644
--- a/src/collision/shapes/SphereShape.cpp
+++ b/src/collision/shapes/SphereShape.cpp
@@ -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){
diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h
index d1492a4a..489adf73 100644
--- a/src/collision/shapes/SphereShape.h
+++ b/src/collision/shapes/SphereShape.h
@@ -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;
diff --git a/src/constraint/SliderJoint.cpp b/src/constraint/SliderJoint.cpp
index fe7ed7c3..b3cfbb14 100644
--- a/src/constraint/SliderJoint.cpp
+++ b/src/constraint/SliderJoint.cpp
@@ -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();
diff --git a/src/engine/CollisionWorld.cpp b/src/engine/CollisionWorld.cpp
index c1defb5a..c15f70d1 100644
--- a/src/engine/CollisionWorld.cpp
+++ b/src/engine/CollisionWorld.cpp
@@ -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());
diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h
index 1f62ed21..c427d3d1 100644
--- a/src/engine/CollisionWorld.h
+++ b/src/engine/CollisionWorld.h
@@ -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);
diff --git a/src/engine/ConstraintSolver.cpp b/src/engine/ConstraintSolver.cpp
index cdd97b41..9e24c6af 100644
--- a/src/engine/ConstraintSolver.cpp
+++ b/src/engine/ConstraintSolver.cpp
@@ -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);
diff --git a/src/engine/ConstraintSolver.h b/src/engine/ConstraintSolver.h
index f15f72cc..ab5dcc01 100644
--- a/src/engine/ConstraintSolver.h
+++ b/src/engine/ConstraintSolver.h
@@ -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;
}
}
diff --git a/src/engine/DynamicsWorld.cpp b/src/engine/DynamicsWorld.cpp
index eb6b520b..d3554624 100644
--- a/src/engine/DynamicsWorld.cpp
+++ b/src/engine/DynamicsWorld.cpp
@@ -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;
diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h
index bef110b5..928eb111 100644
--- a/src/engine/DynamicsWorld.h
+++ b/src/engine/DynamicsWorld.h
@@ -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)
diff --git a/src/engine/OverlappingPair.cpp b/src/engine/OverlappingPair.cpp
index 69a459bb..915c40dd 100644
--- a/src/engine/OverlappingPair.cpp
+++ b/src/engine/OverlappingPair.cpp
@@ -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) {
}
diff --git a/src/engine/OverlappingPair.h b/src/engine/OverlappingPair.h
index 921dba1d..81290ad2 100644
--- a/src/engine/OverlappingPair.h
+++ b/src/engine/OverlappingPair.h
@@ -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