continue to replace SAP broad-phase by a dynamic AABB tree
This commit is contained in:
parent
a448334013
commit
aa76c85e60
|
@ -43,10 +43,6 @@ SET (REACTPHYSICS3D_SOURCES
|
||||||
"src/body/RigidBody.cpp"
|
"src/body/RigidBody.cpp"
|
||||||
"src/collision/broadphase/BroadPhaseAlgorithm.h"
|
"src/collision/broadphase/BroadPhaseAlgorithm.h"
|
||||||
"src/collision/broadphase/BroadPhaseAlgorithm.cpp"
|
"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.h"
|
||||||
"src/collision/broadphase/DynamicAABBTree.cpp"
|
"src/collision/broadphase/DynamicAABBTree.cpp"
|
||||||
"src/collision/narrowphase/EPA/EdgeEPA.h"
|
"src/collision/narrowphase/EPA/EdgeEPA.h"
|
||||||
|
|
|
@ -85,8 +85,11 @@ Box::Box(const openglframework::Vector3& size, const openglframework::Vector3 &p
|
||||||
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
|
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
|
||||||
rp3d::Transform transform(initPosition, initOrientation);
|
rp3d::Transform transform(initPosition, initOrientation);
|
||||||
|
|
||||||
// Create a rigid body corresponding to the cube in the dynamics world
|
// Create a rigid body in the dynamics world
|
||||||
mRigidBody = dynamicsWorld->createRigidBody(transform, mass, collisionShape);
|
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 the Vertex Buffer object has not been created yet
|
||||||
if (!areVBOsCreated) {
|
if (!areVBOsCreated) {
|
||||||
|
|
|
@ -58,8 +58,11 @@ Capsule::Capsule(float radius, float height, const openglframework::Vector3& pos
|
||||||
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
|
rp3d::Quaternion initOrientation = rp3d::Quaternion::identity();
|
||||||
rp3d::Transform transform(initPosition, initOrientation);
|
rp3d::Transform transform(initPosition, initOrientation);
|
||||||
|
|
||||||
// Create a rigid body corresponding to the sphere in the dynamics world
|
// Create a rigid body corresponding 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
|
// Destructor
|
||||||
|
|
|
@ -59,7 +59,10 @@ Cone::Cone(float radius, float height, const openglframework::Vector3 &position,
|
||||||
rp3d::Transform transform(initPosition, initOrientation);
|
rp3d::Transform transform(initPosition, initOrientation);
|
||||||
|
|
||||||
// Create a rigid body corresponding to the cone in the dynamics world
|
// 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
|
// Destructor
|
||||||
|
|
|
@ -72,7 +72,10 @@ ConvexMesh::ConvexMesh(const openglframework::Vector3 &position, float mass,
|
||||||
rp3d::Transform transform(initPosition, initOrientation);
|
rp3d::Transform transform(initPosition, initOrientation);
|
||||||
|
|
||||||
// Create a rigid body corresponding to the sphere in the dynamics world
|
// 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
|
// Destructor
|
||||||
|
|
|
@ -59,7 +59,10 @@ Cylinder::Cylinder(float radius, float height, const openglframework::Vector3 &p
|
||||||
rp3d::Transform transform(initPosition, initOrientation);
|
rp3d::Transform transform(initPosition, initOrientation);
|
||||||
|
|
||||||
// Create a rigid body corresponding to the cylinder in the dynamics world
|
// 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
|
// Destructor
|
||||||
|
|
|
@ -59,7 +59,10 @@ Sphere::Sphere(float radius, const openglframework::Vector3 &position,
|
||||||
rp3d::Transform transform(initPosition, initOrientation);
|
rp3d::Transform transform(initPosition, initOrientation);
|
||||||
|
|
||||||
// Create a rigid body corresponding to the sphere in the dynamics world
|
// 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
|
// Destructor
|
||||||
|
|
|
@ -62,7 +62,7 @@ CollisionBody::~CollisionBody() {
|
||||||
/// This method will return a pointer to the proxy collision shape that links the body with
|
/// This method will return a pointer to the proxy collision shape that links the body with
|
||||||
/// the collision shape you have added.
|
/// the collision shape you have added.
|
||||||
const ProxyShape* CollisionBody::addCollisionShape(const CollisionShape& collisionShape,
|
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)
|
// Create an internal copy of the collision shape into the world (if it does not exist yet)
|
||||||
CollisionShape* newCollisionShape = mWorld.createCollisionShape(collisionShape);
|
CollisionShape* newCollisionShape = mWorld.createCollisionShape(collisionShape);
|
||||||
|
@ -90,14 +90,15 @@ const ProxyShape* CollisionBody::addCollisionShape(const CollisionShape& collisi
|
||||||
}
|
}
|
||||||
|
|
||||||
// Remove a collision shape from the body
|
// Remove a collision shape from the body
|
||||||
void CollisionBody::removeCollisionShape(const CollisionShape* collisionShape) {
|
void CollisionBody::removeCollisionShape(const ProxyShape* proxyShape) {
|
||||||
|
|
||||||
ProxyShape* current = mProxyCollisionShapes;
|
ProxyShape* current = mProxyCollisionShapes;
|
||||||
|
|
||||||
// If the the first proxy shape is the one to remove
|
// If the the first proxy shape is the one to remove
|
||||||
if (current->getCollisionShape() == collisionShape) {
|
if (current == proxyShape) {
|
||||||
mProxyCollisionShapes = current->mNext;
|
mProxyCollisionShapes = current->mNext;
|
||||||
mWorld.mCollisionDetection.removeProxyCollisionShape(current);
|
mWorld.mCollisionDetection.removeProxyCollisionShape(current);
|
||||||
|
mWorld.removeCollisionShape(proxyShape->getInternalCollisionShape());
|
||||||
size_t sizeBytes = current->getSizeInBytes();
|
size_t sizeBytes = current->getSizeInBytes();
|
||||||
current->ProxyShape::~ProxyShape();
|
current->ProxyShape::~ProxyShape();
|
||||||
mWorld.mMemoryAllocator.release(current, sizeBytes);
|
mWorld.mMemoryAllocator.release(current, sizeBytes);
|
||||||
|
@ -109,12 +110,13 @@ void CollisionBody::removeCollisionShape(const CollisionShape* collisionShape) {
|
||||||
while(current->mNext != NULL) {
|
while(current->mNext != NULL) {
|
||||||
|
|
||||||
// If we have found the collision shape to remove
|
// If we have found the collision shape to remove
|
||||||
if (current->mNext->getCollisionShape() == collisionShape) {
|
if (current->mNext == proxyShape) {
|
||||||
|
|
||||||
// Remove the proxy collision shape
|
// Remove the proxy collision shape
|
||||||
ProxyShape* elementToRemove = current->mNext;
|
ProxyShape* elementToRemove = current->mNext;
|
||||||
current->mNext = elementToRemove->mNext;
|
current->mNext = elementToRemove->mNext;
|
||||||
mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove);
|
mWorld.mCollisionDetection.removeProxyCollisionShape(elementToRemove);
|
||||||
|
mWorld.removeCollisionShape(proxyShape->getInternalCollisionShape());
|
||||||
size_t sizeBytes = elementToRemove->getSizeInBytes();
|
size_t sizeBytes = elementToRemove->getSizeInBytes();
|
||||||
elementToRemove->ProxyShape::~ProxyShape();
|
elementToRemove->ProxyShape::~ProxyShape();
|
||||||
mWorld.mMemoryAllocator.release(elementToRemove, sizeBytes);
|
mWorld.mMemoryAllocator.release(elementToRemove, sizeBytes);
|
||||||
|
@ -140,6 +142,7 @@ void CollisionBody::removeAllCollisionShapes() {
|
||||||
// Remove the proxy collision shape
|
// Remove the proxy collision shape
|
||||||
ProxyShape* nextElement = current->mNext;
|
ProxyShape* nextElement = current->mNext;
|
||||||
mWorld.mCollisionDetection.removeProxyCollisionShape(current);
|
mWorld.mCollisionDetection.removeProxyCollisionShape(current);
|
||||||
|
mWorld.removeCollisionShape(current->getInternalCollisionShape());
|
||||||
current->ProxyShape::~ProxyShape();
|
current->ProxyShape::~ProxyShape();
|
||||||
mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape));
|
mWorld.mMemoryAllocator.release(current, sizeof(ProxyShape));
|
||||||
|
|
||||||
|
|
|
@ -137,10 +137,10 @@ class CollisionBody : public Body {
|
||||||
|
|
||||||
/// Add a collision shape to the body.
|
/// Add a collision shape to the body.
|
||||||
const ProxyShape* addCollisionShape(const CollisionShape& collisionShape,
|
const ProxyShape* addCollisionShape(const CollisionShape& collisionShape,
|
||||||
const Transform& transform = Transform::identity());
|
const Transform& transform = Transform::identity());
|
||||||
|
|
||||||
/// Remove a collision shape from the body
|
/// 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
|
/// Return the interpolated transform for rendering
|
||||||
Transform getInterpolatedTransform() const;
|
Transform getInterpolatedTransform() const;
|
||||||
|
@ -159,6 +159,7 @@ class CollisionBody : public Body {
|
||||||
|
|
||||||
// -------------------- Friendship -------------------- //
|
// -------------------- Friendship -------------------- //
|
||||||
|
|
||||||
|
friend class CollisionWorld;
|
||||||
friend class DynamicsWorld;
|
friend class DynamicsWorld;
|
||||||
friend class CollisionDetection;
|
friend class CollisionDetection;
|
||||||
friend class BroadPhaseAlgorithm;
|
friend class BroadPhaseAlgorithm;
|
||||||
|
|
|
@ -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
|
/// 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
|
/// 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
|
/// 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
|
/// 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 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
|
const ProxyShape* RigidBody::addCollisionShape(const CollisionShape& collisionShape,
|
||||||
/// total mass of the rigid body and its inertia tensor). The mass must be positive.
|
decimal mass,
|
||||||
const CollisionShape* RigidBody::addCollisionShape(const CollisionShape& collisionShape,
|
const Transform& transform) {
|
||||||
decimal mass,
|
|
||||||
const Transform& transform
|
|
||||||
) {
|
|
||||||
|
|
||||||
assert(mass > decimal(0.0));
|
assert(mass > decimal(0.0));
|
||||||
|
|
||||||
|
@ -167,15 +166,15 @@ const CollisionShape* RigidBody::addCollisionShape(const CollisionShape& collisi
|
||||||
// collision shape
|
// collision shape
|
||||||
recomputeMassInformation();
|
recomputeMassInformation();
|
||||||
|
|
||||||
// Return a pointer to the collision shape
|
// Return a pointer to the proxy collision shape
|
||||||
return newCollisionShape;
|
return proxyShape;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Remove a collision shape from the body
|
// Remove a collision shape from the body
|
||||||
void RigidBody::removeCollisionShape(const CollisionShape* collisionShape) {
|
void RigidBody::removeCollisionShape(const ProxyShape* proxyCollisionShape) {
|
||||||
|
|
||||||
// Remove the collision shape
|
// Remove the collision shape
|
||||||
CollisionBody::removeCollisionShape(collisionShape);
|
CollisionBody::removeCollisionShape(proxyCollisionShape);
|
||||||
|
|
||||||
// Recompute the total mass, center of mass and inertia tensor
|
// Recompute the total mass, center of mass and inertia tensor
|
||||||
recomputeMassInformation();
|
recomputeMassInformation();
|
||||||
|
|
|
@ -195,12 +195,12 @@ class RigidBody : public CollisionBody {
|
||||||
void applyTorque(const Vector3& torque);
|
void applyTorque(const Vector3& torque);
|
||||||
|
|
||||||
/// Add a collision shape to the body.
|
/// Add a collision shape to the body.
|
||||||
const CollisionShape* addCollisionShape(const CollisionShape& collisionShape,
|
const ProxyShape* addCollisionShape(const CollisionShape& collisionShape,
|
||||||
decimal mass,
|
decimal mass,
|
||||||
const Transform& transform = Transform::identity());
|
const Transform& transform = Transform::identity());
|
||||||
|
|
||||||
/// Remove a collision shape from the body
|
/// 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
|
/// Recompute the center of mass, total mass and inertia tensor of the body using all
|
||||||
/// the collision shapes attached to the body.
|
/// the collision shapes attached to the body.
|
||||||
|
|
|
@ -26,8 +26,6 @@
|
||||||
// Libraries
|
// Libraries
|
||||||
#include "CollisionDetection.h"
|
#include "CollisionDetection.h"
|
||||||
#include "../engine/CollisionWorld.h"
|
#include "../engine/CollisionWorld.h"
|
||||||
#include "broadphase/SweepAndPruneAlgorithm.h"
|
|
||||||
#include "broadphase/NoBroadPhaseAlgorithm.h"
|
|
||||||
#include "../body/Body.h"
|
#include "../body/Body.h"
|
||||||
#include "../collision/shapes/BoxShape.h"
|
#include "../collision/shapes/BoxShape.h"
|
||||||
#include "../body/RigidBody.h"
|
#include "../body/RigidBody.h"
|
||||||
|
@ -101,7 +99,7 @@ void CollisionDetection::computeNarrowPhase() {
|
||||||
CollisionBody* const body2 = shape2->getBody();
|
CollisionBody* const body2 = shape2->getBody();
|
||||||
|
|
||||||
// Update the contact cache of the overlapping pair
|
// 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
|
// Check if the two bodies are allowed to collide, otherwise, we do not test for collision
|
||||||
if (body1->getType() != DYNAMIC && body2->getType() != DYNAMIC) continue;
|
if (body1->getType() != DYNAMIC && body2->getType() != DYNAMIC) continue;
|
||||||
|
@ -123,9 +121,7 @@ void CollisionDetection::computeNarrowPhase() {
|
||||||
// if there really is a collision
|
// if there really is a collision
|
||||||
const Transform transform1 = body1->getTransform() * shape1->getLocalToBodyTransform();
|
const Transform transform1 = body1->getTransform() * shape1->getLocalToBodyTransform();
|
||||||
const Transform transform2 = body2->getTransform() * shape2->getLocalToBodyTransform();
|
const Transform transform2 = body2->getTransform() * shape2->getLocalToBodyTransform();
|
||||||
if (narrowPhaseAlgorithm.testCollision(shape1->getCollisionShape(), transform1,
|
if (narrowPhaseAlgorithm.testCollision(shape1, shape2, contactInfo)) {
|
||||||
shape2->getCollisionShape(), transform2,
|
|
||||||
contactInfo)) {
|
|
||||||
assert(contactInfo != NULL);
|
assert(contactInfo != NULL);
|
||||||
|
|
||||||
// Set the bodies of the contact
|
// Set the bodies of the contact
|
||||||
|
@ -134,8 +130,8 @@ void CollisionDetection::computeNarrowPhase() {
|
||||||
assert(contactInfo->body1 != NULL);
|
assert(contactInfo->body1 != NULL);
|
||||||
assert(contactInfo->body2 != NULL);
|
assert(contactInfo->body2 != NULL);
|
||||||
|
|
||||||
// Notify the world about the new narrow-phase contact
|
// Create a new contact
|
||||||
mWorld->notifyNewContact(pair, contactInfo);
|
createContact(pair, contactInfo);
|
||||||
|
|
||||||
// Delete and remove the contact info from the memory allocator
|
// Delete and remove the contact info from the memory allocator
|
||||||
contactInfo->ContactPointInfo::~ContactPointInfo();
|
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
|
// Remove a body from the collision detection
|
||||||
void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
|
void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
|
||||||
|
|
||||||
// Remove all the overlapping pairs involving this proxy shape
|
// Remove all the overlapping pairs involving this proxy shape
|
||||||
std::map<overlappingpairid, OverlappingPair*>::iterator it;
|
std::map<overlappingpairid, OverlappingPair*>::iterator it;
|
||||||
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
|
for (it = mOverlappingPairs.begin(); it != mOverlappingPairs.end(); ) {
|
||||||
if (it->second->getShape1()->getBroadPhaseID() == proxyShape->getBroadPhaseID() ||
|
if (it->second->getShape1()->mBroadPhaseID == proxyShape->mBroadPhaseID||
|
||||||
it->second->getShape2()->getBroadPhaseID() == proxyShape->getBroadPhaseID()) {
|
it->second->getShape2()->mBroadPhaseID == proxyShape->mBroadPhaseID) {
|
||||||
std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
|
std::map<overlappingpairid, OverlappingPair*>::iterator itToRemove = it;
|
||||||
++it;
|
++it;
|
||||||
|
|
||||||
|
// Destroy the overlapping pair
|
||||||
|
itToRemove->second->OverlappingPair::~OverlappingPair();
|
||||||
|
mWorld->mMemoryAllocator.release(itToRemove->second, sizeof(OverlappingPair));
|
||||||
mOverlappingPairs.erase(itToRemove);
|
mOverlappingPairs.erase(itToRemove);
|
||||||
}
|
}
|
||||||
else {
|
else {
|
||||||
|
@ -226,3 +205,58 @@ void CollisionDetection::removeProxyCollisionShape(ProxyShape* proxyShape) {
|
||||||
// Remove the body from the broad-phase
|
// Remove the body from the broad-phase
|
||||||
mBroadPhaseAlgorithm.removeProxyCollisionShape(proxyShape);
|
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;
|
||||||
|
}
|
||||||
|
|
|
@ -80,6 +80,10 @@ class CollisionDetection {
|
||||||
/// True if some collision shapes have been added previously
|
/// True if some collision shapes have been added previously
|
||||||
bool mIsCollisionShapesAdded;
|
bool mIsCollisionShapesAdded;
|
||||||
|
|
||||||
|
/// All the contact constraints
|
||||||
|
// TODO : Remove this variable (we will use the ones in the island now)
|
||||||
|
std::vector<ContactManifold*> mContactManifolds;
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
/// Private copy-constructor
|
/// Private copy-constructor
|
||||||
|
@ -98,8 +102,13 @@ class CollisionDetection {
|
||||||
NarrowPhaseAlgorithm& SelectNarrowPhaseAlgorithm(const CollisionShape* collisionShape1,
|
NarrowPhaseAlgorithm& SelectNarrowPhaseAlgorithm(const CollisionShape* collisionShape1,
|
||||||
const CollisionShape* collisionShape2);
|
const CollisionShape* collisionShape2);
|
||||||
|
|
||||||
/// Remove an overlapping pair if it is not overlapping anymore
|
/// Create a new contact
|
||||||
void removeOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
|
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 :
|
public :
|
||||||
|
|
||||||
|
@ -131,6 +140,11 @@ class CollisionDetection {
|
||||||
|
|
||||||
/// Allow the broadphase to notify the collision detection about an overlapping pair.
|
/// Allow the broadphase to notify the collision detection about an overlapping pair.
|
||||||
void broadPhaseNotifyOverlappingPair(ProxyShape* shape1, ProxyShape* shape2);
|
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
|
// Select the narrow-phase collision algorithm to use given two collision shapes
|
||||||
|
|
|
@ -25,6 +25,7 @@
|
||||||
|
|
||||||
// Libraries
|
// Libraries
|
||||||
#include "BroadPhaseAlgorithm.h"
|
#include "BroadPhaseAlgorithm.h"
|
||||||
|
#include "../CollisionDetection.h"
|
||||||
|
|
||||||
// We want to use the ReactPhysics3D namespace
|
// We want to use the ReactPhysics3D namespace
|
||||||
using namespace reactphysics3d;
|
using namespace reactphysics3d;
|
||||||
|
@ -33,7 +34,7 @@ using namespace reactphysics3d;
|
||||||
BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
|
BroadPhaseAlgorithm::BroadPhaseAlgorithm(CollisionDetection& collisionDetection)
|
||||||
:mDynamicAABBTree(*this), mNbMovedShapes(0), mNbAllocatedMovedShapes(8),
|
:mDynamicAABBTree(*this), mNbMovedShapes(0), mNbAllocatedMovedShapes(8),
|
||||||
mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8),
|
mNbNonUsedMovedShapes(0), mNbPotentialPairs(0), mNbAllocatedPotentialPairs(8),
|
||||||
mPairManager(collisionDetection), mCollisionDetection(collisionDetection) {
|
mCollisionDetection(collisionDetection) {
|
||||||
|
|
||||||
// Allocate memory for the array of non-static bodies IDs
|
// Allocate memory for the array of non-static bodies IDs
|
||||||
mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
|
mMovedShapes = (int*) malloc(mNbAllocatedMovedShapes * sizeof(int));
|
||||||
|
|
|
@ -29,7 +29,6 @@
|
||||||
// Libraries
|
// Libraries
|
||||||
#include <vector>
|
#include <vector>
|
||||||
#include "../../body/CollisionBody.h"
|
#include "../../body/CollisionBody.h"
|
||||||
#include "PairManager.h"
|
|
||||||
#include "DynamicAABBTree.h"
|
#include "DynamicAABBTree.h"
|
||||||
|
|
||||||
/// Namespace ReactPhysics3D
|
/// Namespace ReactPhysics3D
|
||||||
|
@ -110,9 +109,6 @@ class BroadPhaseAlgorithm {
|
||||||
/// Number of allocated elements for the array of potential overlapping pairs
|
/// Number of allocated elements for the array of potential overlapping pairs
|
||||||
uint mNbAllocatedPotentialPairs;
|
uint mNbAllocatedPotentialPairs;
|
||||||
|
|
||||||
/// Pair manager containing the overlapping pairs
|
|
||||||
PairManager mPairManager;
|
|
||||||
|
|
||||||
/// Reference to the collision detection object
|
/// Reference to the collision detection object
|
||||||
CollisionDetection& mCollisionDetection;
|
CollisionDetection& mCollisionDetection;
|
||||||
|
|
||||||
|
@ -156,14 +152,6 @@ class BroadPhaseAlgorithm {
|
||||||
|
|
||||||
/// Compute all the overlapping pairs of collision shapes
|
/// Compute all the overlapping pairs of collision shapes
|
||||||
void computeOverlappingPairs();
|
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
|
// 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 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
|
#endif
|
||||||
|
|
|
@ -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() {
|
|
||||||
|
|
||||||
}
|
|
|
@ -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
|
|
||||||
|
|
||||||
|
|
|
@ -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;
|
|
||||||
}
|
|
|
@ -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
|
|
||||||
|
|
||||||
|
|
|
@ -63,7 +63,7 @@ void BoxShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const
|
||||||
}
|
}
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
ProxyBoxShape::ProxyBoxShape(const BoxShape* shape, CollisionBody* body,
|
ProxyBoxShape::ProxyBoxShape(BoxShape* shape, CollisionBody* body,
|
||||||
const Transform& transform, decimal mass)
|
const Transform& transform, decimal mass)
|
||||||
:ProxyShape(body, transform, mass), mCollisionShape(shape){
|
:ProxyShape(body, transform, mass), mCollisionShape(shape){
|
||||||
|
|
||||||
|
|
|
@ -102,7 +102,7 @@ class BoxShape : public CollisionShape {
|
||||||
|
|
||||||
/// Create a proxy collision shape for the collision shape
|
/// Create a proxy collision shape for the collision shape
|
||||||
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
||||||
const Transform& transform, decimal mass) const;
|
const Transform& transform, decimal mass);
|
||||||
};
|
};
|
||||||
|
|
||||||
// Class ProxyBoxShape
|
// Class ProxyBoxShape
|
||||||
|
@ -116,7 +116,7 @@ class ProxyBoxShape : public ProxyShape {
|
||||||
// -------------------- Attributes -------------------- //
|
// -------------------- Attributes -------------------- //
|
||||||
|
|
||||||
/// Pointer to the actual collision shape
|
/// Pointer to the actual collision shape
|
||||||
const BoxShape* mCollisionShape;
|
BoxShape* mCollisionShape;
|
||||||
|
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
@ -127,12 +127,15 @@ class ProxyBoxShape : public ProxyShape {
|
||||||
/// Private assignment operator
|
/// Private assignment operator
|
||||||
ProxyBoxShape& operator=(const ProxyBoxShape& proxyShape);
|
ProxyBoxShape& operator=(const ProxyBoxShape& proxyShape);
|
||||||
|
|
||||||
|
/// Return the non-const collision shape
|
||||||
|
virtual CollisionShape* getInternalCollisionShape() const;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
/// Constructor
|
/// Constructor
|
||||||
ProxyBoxShape(const BoxShape* shape, CollisionBody* body,
|
ProxyBoxShape(BoxShape* shape, CollisionBody* body,
|
||||||
const Transform& transform, decimal mass);
|
const Transform& transform, decimal mass);
|
||||||
|
|
||||||
/// Destructor
|
/// Destructor
|
||||||
|
@ -206,11 +209,16 @@ inline bool BoxShape::isEqualTo(const CollisionShape& otherCollisionShape) const
|
||||||
|
|
||||||
// Create a proxy collision shape for the collision shape
|
// Create a proxy collision shape for the collision shape
|
||||||
inline ProxyShape* BoxShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
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,
|
return new (allocator.allocate(sizeof(ProxyBoxShape))) ProxyBoxShape(this, body,
|
||||||
transform, mass);
|
transform, mass);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Return the non-const collision shape
|
||||||
|
inline CollisionShape* ProxyBoxShape::getInternalCollisionShape() const {
|
||||||
|
return mCollisionShape;
|
||||||
|
}
|
||||||
|
|
||||||
// Return the collision shape
|
// Return the collision shape
|
||||||
inline const CollisionShape* ProxyBoxShape::getCollisionShape() const {
|
inline const CollisionShape* ProxyBoxShape::getCollisionShape() const {
|
||||||
return mCollisionShape;
|
return mCollisionShape;
|
||||||
|
|
|
@ -125,7 +125,7 @@ void CapsuleShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) co
|
||||||
}
|
}
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
ProxyCapsuleShape::ProxyCapsuleShape(const CapsuleShape* shape, CollisionBody* body,
|
ProxyCapsuleShape::ProxyCapsuleShape(CapsuleShape* shape, CollisionBody* body,
|
||||||
const Transform& transform, decimal mass)
|
const Transform& transform, decimal mass)
|
||||||
:ProxyShape(body, transform, mass), mCollisionShape(shape){
|
:ProxyShape(body, transform, mass), mCollisionShape(shape){
|
||||||
|
|
||||||
|
|
|
@ -102,7 +102,7 @@ class CapsuleShape : public CollisionShape {
|
||||||
|
|
||||||
/// Create a proxy collision shape for the collision shape
|
/// Create a proxy collision shape for the collision shape
|
||||||
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
||||||
const Transform& transform, decimal mass) const;
|
const Transform& transform, decimal mass);
|
||||||
};
|
};
|
||||||
|
|
||||||
// Class ProxyCapsuleShape
|
// Class ProxyCapsuleShape
|
||||||
|
@ -116,7 +116,7 @@ class ProxyCapsuleShape : public ProxyShape {
|
||||||
// -------------------- Attributes -------------------- //
|
// -------------------- Attributes -------------------- //
|
||||||
|
|
||||||
/// Pointer to the actual collision shape
|
/// Pointer to the actual collision shape
|
||||||
const CapsuleShape* mCollisionShape;
|
CapsuleShape* mCollisionShape;
|
||||||
|
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
@ -127,12 +127,15 @@ class ProxyCapsuleShape : public ProxyShape {
|
||||||
/// Private assignment operator
|
/// Private assignment operator
|
||||||
ProxyCapsuleShape& operator=(const ProxyCapsuleShape& proxyShape);
|
ProxyCapsuleShape& operator=(const ProxyCapsuleShape& proxyShape);
|
||||||
|
|
||||||
|
/// Return the non-const collision shape
|
||||||
|
virtual CollisionShape* getInternalCollisionShape() const;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
/// Constructor
|
/// Constructor
|
||||||
ProxyCapsuleShape(const CapsuleShape* shape, CollisionBody* body,
|
ProxyCapsuleShape(CapsuleShape* shape, CollisionBody* body,
|
||||||
const Transform& transform, decimal mass);
|
const Transform& transform, decimal mass);
|
||||||
|
|
||||||
/// Destructor
|
/// Destructor
|
||||||
|
@ -197,11 +200,16 @@ inline bool CapsuleShape::isEqualTo(const CollisionShape& otherCollisionShape) c
|
||||||
|
|
||||||
// Create a proxy collision shape for the collision shape
|
// Create a proxy collision shape for the collision shape
|
||||||
inline ProxyShape* CapsuleShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
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,
|
return new (allocator.allocate(sizeof(ProxyCapsuleShape))) ProxyCapsuleShape(this, body,
|
||||||
transform, mass);
|
transform, mass);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Return the non-const collision shape
|
||||||
|
inline CollisionShape* ProxyCapsuleShape::getInternalCollisionShape() const {
|
||||||
|
return mCollisionShape;
|
||||||
|
}
|
||||||
|
|
||||||
// Return the collision shape
|
// Return the collision shape
|
||||||
inline const CollisionShape* ProxyCapsuleShape::getCollisionShape() const {
|
inline const CollisionShape* ProxyCapsuleShape::getCollisionShape() const {
|
||||||
return mCollisionShape;
|
return mCollisionShape;
|
||||||
|
|
|
@ -120,7 +120,7 @@ class CollisionShape {
|
||||||
|
|
||||||
/// Create a proxy collision shape for the collision shape
|
/// Create a proxy collision shape for the collision shape
|
||||||
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
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
|
/// Private assignment operator
|
||||||
ProxyShape& operator=(const ProxyShape& proxyShape);
|
ProxyShape& operator=(const ProxyShape& proxyShape);
|
||||||
|
|
||||||
|
/// Return the non-const collision shape
|
||||||
|
virtual CollisionShape* getInternalCollisionShape() const=0;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
@ -204,6 +207,7 @@ class ProxyShape {
|
||||||
friend class RigidBody;
|
friend class RigidBody;
|
||||||
friend class BroadPhaseAlgorithm;
|
friend class BroadPhaseAlgorithm;
|
||||||
friend class DynamicAABBTree;
|
friend class DynamicAABBTree;
|
||||||
|
friend class CollisionDetection;
|
||||||
};
|
};
|
||||||
|
|
||||||
// Return the type of the collision shape
|
// Return the type of the collision shape
|
||||||
|
|
|
@ -94,7 +94,7 @@ Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction) c
|
||||||
}
|
}
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
ProxyConeShape::ProxyConeShape(const ConeShape* shape, CollisionBody* body,
|
ProxyConeShape::ProxyConeShape(ConeShape* shape, CollisionBody* body,
|
||||||
const Transform& transform, decimal mass)
|
const Transform& transform, decimal mass)
|
||||||
:ProxyShape(body, transform, mass), mCollisionShape(shape){
|
:ProxyShape(body, transform, mass), mCollisionShape(shape){
|
||||||
|
|
||||||
|
|
|
@ -110,7 +110,7 @@ class ConeShape : public CollisionShape {
|
||||||
|
|
||||||
/// Create a proxy collision shape for the collision shape
|
/// Create a proxy collision shape for the collision shape
|
||||||
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
||||||
const Transform& transform, decimal mass) const;
|
const Transform& transform, decimal mass);
|
||||||
};
|
};
|
||||||
|
|
||||||
// Class ProxyConeShape
|
// Class ProxyConeShape
|
||||||
|
@ -124,7 +124,7 @@ class ProxyConeShape : public ProxyShape {
|
||||||
// -------------------- Attributes -------------------- //
|
// -------------------- Attributes -------------------- //
|
||||||
|
|
||||||
/// Pointer to the actual collision shape
|
/// Pointer to the actual collision shape
|
||||||
const ConeShape* mCollisionShape;
|
ConeShape* mCollisionShape;
|
||||||
|
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
@ -135,12 +135,15 @@ class ProxyConeShape : public ProxyShape {
|
||||||
/// Private assignment operator
|
/// Private assignment operator
|
||||||
ProxyConeShape& operator=(const ProxyConeShape& proxyShape);
|
ProxyConeShape& operator=(const ProxyConeShape& proxyShape);
|
||||||
|
|
||||||
|
/// Return the non-const collision shape
|
||||||
|
virtual CollisionShape* getInternalCollisionShape() const;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
/// Constructor
|
/// Constructor
|
||||||
ProxyConeShape(const ConeShape* shape, CollisionBody* body,
|
ProxyConeShape(ConeShape* shape, CollisionBody* body,
|
||||||
const Transform& transform, decimal mass);
|
const Transform& transform, decimal mass);
|
||||||
|
|
||||||
/// Destructor
|
/// Destructor
|
||||||
|
@ -213,11 +216,16 @@ inline bool ConeShape::isEqualTo(const CollisionShape& otherCollisionShape) cons
|
||||||
|
|
||||||
// Create a proxy collision shape for the collision shape
|
// Create a proxy collision shape for the collision shape
|
||||||
inline ProxyShape* ConeShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
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,
|
return new (allocator.allocate(sizeof(ProxyConeShape))) ProxyConeShape(this, body,
|
||||||
transform, mass);
|
transform, mass);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Return the non-const collision shape
|
||||||
|
inline CollisionShape* ProxyConeShape::getInternalCollisionShape() const {
|
||||||
|
return mCollisionShape;
|
||||||
|
}
|
||||||
|
|
||||||
// Return the collision shape
|
// Return the collision shape
|
||||||
inline const CollisionShape* ProxyConeShape::getCollisionShape() const {
|
inline const CollisionShape* ProxyConeShape::getCollisionShape() const {
|
||||||
return mCollisionShape;
|
return mCollisionShape;
|
||||||
|
|
|
@ -225,8 +225,8 @@ bool ConvexMeshShape::isEqualTo(const CollisionShape& otherCollisionShape) const
|
||||||
}
|
}
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
ProxyConvexMeshShape::ProxyConvexMeshShape(const ConvexMeshShape* shape, CollisionBody* body,
|
ProxyConvexMeshShape::ProxyConvexMeshShape(ConvexMeshShape* shape, CollisionBody* body,
|
||||||
const Transform& transform, decimal mass)
|
const Transform& transform, decimal mass)
|
||||||
:ProxyShape(body, transform, mass), mCollisionShape(shape),
|
:ProxyShape(body, transform, mass), mCollisionShape(shape),
|
||||||
mCachedSupportVertex(0) {
|
mCachedSupportVertex(0) {
|
||||||
|
|
||||||
|
|
|
@ -140,7 +140,7 @@ class ConvexMeshShape : public CollisionShape {
|
||||||
|
|
||||||
/// Create a proxy collision shape for the collision shape
|
/// Create a proxy collision shape for the collision shape
|
||||||
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
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 -------------------- //
|
// -------------------- Attributes -------------------- //
|
||||||
|
|
||||||
/// Pointer to the actual collision shape
|
/// Pointer to the actual collision shape
|
||||||
const ConvexMeshShape* mCollisionShape;
|
ConvexMeshShape* mCollisionShape;
|
||||||
|
|
||||||
/// Cached support vertex index (previous support vertex for hill-climbing)
|
/// Cached support vertex index (previous support vertex for hill-climbing)
|
||||||
uint mCachedSupportVertex;
|
uint mCachedSupportVertex;
|
||||||
|
@ -168,12 +168,15 @@ class ProxyConvexMeshShape : public ProxyShape {
|
||||||
/// Private assignment operator
|
/// Private assignment operator
|
||||||
ProxyConvexMeshShape& operator=(const ProxyConvexMeshShape& proxyShape);
|
ProxyConvexMeshShape& operator=(const ProxyConvexMeshShape& proxyShape);
|
||||||
|
|
||||||
|
/// Return the non-const collision shape
|
||||||
|
virtual CollisionShape* getInternalCollisionShape() const;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
/// Constructor
|
/// Constructor
|
||||||
ProxyConvexMeshShape(const ConvexMeshShape* shape, CollisionBody* body,
|
ProxyConvexMeshShape(ConvexMeshShape* shape, CollisionBody* body,
|
||||||
const Transform& transform, decimal mass);
|
const Transform& transform, decimal mass);
|
||||||
|
|
||||||
/// Destructor
|
/// Destructor
|
||||||
|
@ -281,11 +284,16 @@ inline void ConvexMeshShape::setIsEdgesInformationUsed(bool isEdgesUsed) {
|
||||||
inline ProxyShape* ConvexMeshShape::createProxyShape(MemoryAllocator& allocator,
|
inline ProxyShape* ConvexMeshShape::createProxyShape(MemoryAllocator& allocator,
|
||||||
CollisionBody* body,
|
CollisionBody* body,
|
||||||
const Transform& transform,
|
const Transform& transform,
|
||||||
decimal mass) const {
|
decimal mass) {
|
||||||
return new (allocator.allocate(sizeof(ProxyConvexMeshShape))) ProxyConvexMeshShape(this, body,
|
return new (allocator.allocate(sizeof(ProxyConvexMeshShape))) ProxyConvexMeshShape(this, body,
|
||||||
transform, mass);
|
transform, mass);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Return the non-const collision shape
|
||||||
|
inline CollisionShape* ProxyConvexMeshShape::getInternalCollisionShape() const {
|
||||||
|
return mCollisionShape;
|
||||||
|
}
|
||||||
|
|
||||||
// Return the collision shape
|
// Return the collision shape
|
||||||
inline const CollisionShape* ProxyConvexMeshShape::getCollisionShape() const {
|
inline const CollisionShape* ProxyConvexMeshShape::getCollisionShape() const {
|
||||||
return mCollisionShape;
|
return mCollisionShape;
|
||||||
|
|
|
@ -87,7 +87,7 @@ Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& directio
|
||||||
}
|
}
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
ProxyCylinderShape::ProxyCylinderShape(const CylinderShape* cylinderShape, CollisionBody* body,
|
ProxyCylinderShape::ProxyCylinderShape(CylinderShape* cylinderShape, CollisionBody* body,
|
||||||
const Transform& transform, decimal mass)
|
const Transform& transform, decimal mass)
|
||||||
:ProxyShape(body, transform, mass), mCollisionShape(cylinderShape){
|
:ProxyShape(body, transform, mass), mCollisionShape(cylinderShape){
|
||||||
|
|
||||||
|
|
|
@ -107,7 +107,7 @@ class CylinderShape : public CollisionShape {
|
||||||
|
|
||||||
/// Create a proxy collision shape for the collision shape
|
/// Create a proxy collision shape for the collision shape
|
||||||
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
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 -------------------- //
|
// -------------------- Attributes -------------------- //
|
||||||
|
|
||||||
/// Pointer to the actual collision shape
|
/// Pointer to the actual collision shape
|
||||||
const CylinderShape* mCollisionShape;
|
CylinderShape* mCollisionShape;
|
||||||
|
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
|
@ -133,12 +132,15 @@ class ProxyCylinderShape : public ProxyShape {
|
||||||
/// Private assignment operator
|
/// Private assignment operator
|
||||||
ProxyCylinderShape& operator=(const ProxyCylinderShape& proxyShape);
|
ProxyCylinderShape& operator=(const ProxyCylinderShape& proxyShape);
|
||||||
|
|
||||||
|
/// Return the non-const collision shape
|
||||||
|
virtual CollisionShape* getInternalCollisionShape() const;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
/// Constructor
|
/// Constructor
|
||||||
ProxyCylinderShape(const CylinderShape* cylinderShape, CollisionBody* body,
|
ProxyCylinderShape(CylinderShape* cylinderShape, CollisionBody* body,
|
||||||
const Transform& transform, decimal mass);
|
const Transform& transform, decimal mass);
|
||||||
|
|
||||||
/// Destructor
|
/// Destructor
|
||||||
|
@ -211,11 +213,16 @@ inline bool CylinderShape::isEqualTo(const CollisionShape& otherCollisionShape)
|
||||||
|
|
||||||
// Create a proxy collision shape for the collision shape
|
// Create a proxy collision shape for the collision shape
|
||||||
inline ProxyShape* CylinderShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
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,
|
return new (allocator.allocate(sizeof(ProxyCylinderShape))) ProxyCylinderShape(this, body,
|
||||||
transform, mass);
|
transform, mass);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Return the non-const collision shape
|
||||||
|
inline CollisionShape* ProxyCylinderShape::getInternalCollisionShape() const {
|
||||||
|
return mCollisionShape;
|
||||||
|
}
|
||||||
|
|
||||||
// Return the collision shape
|
// Return the collision shape
|
||||||
inline const CollisionShape* ProxyCylinderShape::getCollisionShape() const {
|
inline const CollisionShape* ProxyCylinderShape::getCollisionShape() const {
|
||||||
return mCollisionShape;
|
return mCollisionShape;
|
||||||
|
|
|
@ -47,7 +47,7 @@ SphereShape::~SphereShape() {
|
||||||
}
|
}
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
ProxySphereShape::ProxySphereShape(const SphereShape* shape, CollisionBody* body,
|
ProxySphereShape::ProxySphereShape(SphereShape* shape, CollisionBody* body,
|
||||||
const Transform& transform, decimal mass)
|
const Transform& transform, decimal mass)
|
||||||
:ProxyShape(body, transform, mass), mCollisionShape(shape){
|
:ProxyShape(body, transform, mass), mCollisionShape(shape){
|
||||||
|
|
||||||
|
|
|
@ -97,7 +97,7 @@ class SphereShape : public CollisionShape {
|
||||||
|
|
||||||
/// Create a proxy collision shape for the collision shape
|
/// Create a proxy collision shape for the collision shape
|
||||||
virtual ProxyShape* createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
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 -------------------- //
|
// -------------------- Attributes -------------------- //
|
||||||
|
|
||||||
/// Pointer to the actual collision shape
|
/// Pointer to the actual collision shape
|
||||||
const SphereShape* mCollisionShape;
|
SphereShape* mCollisionShape;
|
||||||
|
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
@ -123,12 +123,15 @@ class ProxySphereShape : public ProxyShape {
|
||||||
/// Private assignment operator
|
/// Private assignment operator
|
||||||
ProxySphereShape& operator=(const ProxySphereShape& proxyShape);
|
ProxySphereShape& operator=(const ProxySphereShape& proxyShape);
|
||||||
|
|
||||||
|
/// Return the non-const collision shape
|
||||||
|
virtual CollisionShape* getInternalCollisionShape() const;
|
||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
/// Constructor
|
/// Constructor
|
||||||
ProxySphereShape(const SphereShape* shape, CollisionBody* body,
|
ProxySphereShape(SphereShape* shape, CollisionBody* body,
|
||||||
const Transform& transform, decimal mass);
|
const Transform& transform, decimal mass);
|
||||||
|
|
||||||
/// Destructor
|
/// Destructor
|
||||||
|
@ -229,11 +232,16 @@ inline bool SphereShape::isEqualTo(const CollisionShape& otherCollisionShape) co
|
||||||
|
|
||||||
// Create a proxy collision shape for the collision shape
|
// Create a proxy collision shape for the collision shape
|
||||||
inline ProxyShape* SphereShape::createProxyShape(MemoryAllocator& allocator, CollisionBody* body,
|
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,
|
return new (allocator.allocate(sizeof(ProxySphereShape))) ProxySphereShape(this, body,
|
||||||
transform, mass);
|
transform, mass);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Return the non-const collision shape
|
||||||
|
inline CollisionShape* ProxySphereShape::getInternalCollisionShape() const {
|
||||||
|
return mCollisionShape;
|
||||||
|
}
|
||||||
|
|
||||||
// Return the collision shape
|
// Return the collision shape
|
||||||
inline const CollisionShape* ProxySphereShape::getCollisionShape() const {
|
inline const CollisionShape* ProxySphereShape::getCollisionShape() const {
|
||||||
return mCollisionShape;
|
return mCollisionShape;
|
||||||
|
|
|
@ -76,7 +76,7 @@ void SliderJoint::initBeforeSolve(const ConstraintSolverData& constraintSolverDa
|
||||||
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
|
mIndexBody2 = constraintSolverData.mapBodyToConstrainedVelocityIndex.find(mBody2)->second;
|
||||||
|
|
||||||
// Get the bodies positions and orientations
|
// Get the bodies positions and orientations
|
||||||
const Vector3& x1 = mBody1-mCenterOfMassWorld;
|
const Vector3& x1 = mBody1->mCenterOfMassWorld;
|
||||||
const Vector3& x2 = mBody2->mCenterOfMassWorld;
|
const Vector3& x2 = mBody2->mCenterOfMassWorld;
|
||||||
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
|
const Quaternion& orientationBody1 = mBody1->getTransform().getOrientation();
|
||||||
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
|
const Quaternion& orientationBody2 = mBody2->getTransform().getOrientation();
|
||||||
|
|
|
@ -33,7 +33,8 @@ using namespace std;
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
CollisionWorld::CollisionWorld()
|
CollisionWorld::CollisionWorld()
|
||||||
: mCollisionDetection(this, mMemoryAllocator), mCurrentBodyID(0) {
|
: mCollisionDetection(this, mMemoryAllocator), mCurrentBodyID(0),
|
||||||
|
mEventListener(NULL) {
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -43,21 +44,8 @@ CollisionWorld::~CollisionWorld() {
|
||||||
assert(mBodies.empty());
|
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
|
// Create a collision body and add it to the world
|
||||||
CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform,
|
CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform) {
|
||||||
CollisionShape* collisionShape) {
|
|
||||||
|
|
||||||
// Get the next available body ID
|
// Get the next available body ID
|
||||||
bodyindex bodyID = computeNextAvailableBodyID();
|
bodyindex bodyID = computeNextAvailableBodyID();
|
||||||
|
@ -67,16 +55,13 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform,
|
||||||
|
|
||||||
// Create the collision body
|
// Create the collision body
|
||||||
CollisionBody* collisionBody = new (mMemoryAllocator.allocate(sizeof(CollisionBody)))
|
CollisionBody* collisionBody = new (mMemoryAllocator.allocate(sizeof(CollisionBody)))
|
||||||
CollisionBody(transform, collisionShape, bodyID);
|
CollisionBody(transform, *this, bodyID);
|
||||||
|
|
||||||
assert(collisionBody != NULL);
|
assert(collisionBody != NULL);
|
||||||
|
|
||||||
// Add the collision body to the world
|
// Add the collision body to the world
|
||||||
mBodies.insert(collisionBody);
|
mBodies.insert(collisionBody);
|
||||||
|
|
||||||
// Add the collision body to the collision detection
|
|
||||||
mCollisionDetection.addProxyCollisionShape(collisionBody);
|
|
||||||
|
|
||||||
// Return the pointer to the rigid body
|
// Return the pointer to the rigid body
|
||||||
return collisionBody;
|
return collisionBody;
|
||||||
}
|
}
|
||||||
|
@ -84,8 +69,8 @@ CollisionBody* CollisionWorld::createCollisionBody(const Transform& transform,
|
||||||
// Destroy a collision body
|
// Destroy a collision body
|
||||||
void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
|
void CollisionWorld::destroyCollisionBody(CollisionBody* collisionBody) {
|
||||||
|
|
||||||
// Remove the body from the collision detection
|
// Remove all the collision shapes of the body
|
||||||
mCollisionDetection.removeProxyCollisionShape(collisionBody);
|
collisionBody->removeAllCollisionShapes();
|
||||||
|
|
||||||
// Add the body ID to the list of free IDs
|
// Add the body ID to the list of free IDs
|
||||||
mFreeBodiesIDs.push_back(collisionBody->getID());
|
mFreeBodiesIDs.push_back(collisionBody->getID());
|
||||||
|
|
|
@ -77,6 +77,9 @@ class CollisionWorld {
|
||||||
/// Memory allocator
|
/// Memory allocator
|
||||||
MemoryAllocator mMemoryAllocator;
|
MemoryAllocator mMemoryAllocator;
|
||||||
|
|
||||||
|
/// Pointer to an event listener object
|
||||||
|
EventListener* mEventListener;
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
/// Private copy-constructor
|
/// Private copy-constructor
|
||||||
|
@ -85,13 +88,6 @@ class CollisionWorld {
|
||||||
/// Private assignment operator
|
/// Private assignment operator
|
||||||
CollisionWorld& operator=(const CollisionWorld& world);
|
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
|
/// Return the next available body ID
|
||||||
bodyindex computeNextAvailableBodyID();
|
bodyindex computeNextAvailableBodyID();
|
||||||
|
|
||||||
|
@ -118,8 +114,7 @@ class CollisionWorld {
|
||||||
std::set<CollisionBody*>::iterator getBodiesEndIterator();
|
std::set<CollisionBody*>::iterator getBodiesEndIterator();
|
||||||
|
|
||||||
/// Create a collision body
|
/// Create a collision body
|
||||||
CollisionBody* createCollisionBody(const Transform& transform,
|
CollisionBody* createCollisionBody(const Transform& transform);
|
||||||
CollisionShape* collisionShape);
|
|
||||||
|
|
||||||
/// Destroy a collision body
|
/// Destroy a collision body
|
||||||
void destroyCollisionBody(CollisionBody* collisionBody);
|
void destroyCollisionBody(CollisionBody* collisionBody);
|
||||||
|
|
|
@ -30,14 +30,9 @@
|
||||||
using namespace reactphysics3d;
|
using namespace reactphysics3d;
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
ConstraintSolver::ConstraintSolver(std::vector<Vector3>& positions,
|
ConstraintSolver::ConstraintSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
|
||||||
std::vector<Quaternion>& orientations,
|
: mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
|
||||||
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex)
|
mIsWarmStartingActive(true), mConstraintSolverData(mapBodyToVelocityIndex) {
|
||||||
: mLinearVelocities(NULL), mAngularVelocities(NULL), mPositions(positions),
|
|
||||||
mOrientations(orientations),
|
|
||||||
mMapBodyToConstrainedVelocityIndex(mapBodyToVelocityIndex),
|
|
||||||
mIsWarmStartingActive(true), mConstraintSolverData(positions, orientations,
|
|
||||||
mapBodyToVelocityIndex){
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -51,8 +46,6 @@ void ConstraintSolver::initializeForIsland(decimal dt, Island* island) {
|
||||||
|
|
||||||
PROFILE("ConstraintSolver::initializeForIsland()");
|
PROFILE("ConstraintSolver::initializeForIsland()");
|
||||||
|
|
||||||
assert(mLinearVelocities != NULL);
|
|
||||||
assert(mAngularVelocities != NULL);
|
|
||||||
assert(island != NULL);
|
assert(island != NULL);
|
||||||
assert(island->getNbBodies() > 0);
|
assert(island->getNbBodies() > 0);
|
||||||
assert(island->getNbJoints() > 0);
|
assert(island->getNbJoints() > 0);
|
||||||
|
|
|
@ -55,10 +55,10 @@ struct ConstraintSolverData {
|
||||||
Vector3* angularVelocities;
|
Vector3* angularVelocities;
|
||||||
|
|
||||||
/// Reference to the bodies positions
|
/// Reference to the bodies positions
|
||||||
std::vector<Vector3>& positions;
|
Vector3* positions;
|
||||||
|
|
||||||
/// Reference to the bodies orientations
|
/// Reference to the bodies orientations
|
||||||
std::vector<Quaternion>& orientations;
|
Quaternion* orientations;
|
||||||
|
|
||||||
/// Reference to the map that associates rigid body to their index
|
/// Reference to the map that associates rigid body to their index
|
||||||
/// in the constrained velocities array
|
/// in the constrained velocities array
|
||||||
|
@ -68,12 +68,9 @@ struct ConstraintSolverData {
|
||||||
bool isWarmStartingActive;
|
bool isWarmStartingActive;
|
||||||
|
|
||||||
/// Constructor
|
/// Constructor
|
||||||
ConstraintSolverData(std::vector<Vector3>& refPositions,
|
ConstraintSolverData(const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex)
|
||||||
std::vector<Quaternion>& refOrientations,
|
:linearVelocities(NULL), angularVelocities(NULL),
|
||||||
const std::map<RigidBody*, uint>& refMapBodyToConstrainedVelocityIndex)
|
positions(NULL), orientations(NULL),
|
||||||
:linearVelocities(NULL),
|
|
||||||
angularVelocities(NULL),
|
|
||||||
positions(refPositions), orientations(refOrientations),
|
|
||||||
mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){
|
mapBodyToConstrainedVelocityIndex(refMapBodyToConstrainedVelocityIndex){
|
||||||
|
|
||||||
}
|
}
|
||||||
|
@ -155,20 +152,6 @@ class ConstraintSolver {
|
||||||
|
|
||||||
// -------------------- Attributes -------------------- //
|
// -------------------- 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
|
/// Reference to the map that associates rigid body to their index in
|
||||||
/// the constrained velocities array
|
/// the constrained velocities array
|
||||||
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
|
const std::map<RigidBody*, uint>& mMapBodyToConstrainedVelocityIndex;
|
||||||
|
@ -187,8 +170,7 @@ class ConstraintSolver {
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
/// Constructor
|
/// Constructor
|
||||||
ConstraintSolver(std::vector<Vector3>& positions, std::vector<Quaternion>& orientations,
|
ConstraintSolver(const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
|
||||||
const std::map<RigidBody*, uint>& mapBodyToVelocityIndex);
|
|
||||||
|
|
||||||
/// Destructor
|
/// Destructor
|
||||||
~ConstraintSolver();
|
~ConstraintSolver();
|
||||||
|
@ -211,6 +193,10 @@ class ConstraintSolver {
|
||||||
/// Set the constrained velocities arrays
|
/// Set the constrained velocities arrays
|
||||||
void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
|
void setConstrainedVelocitiesArrays(Vector3* constrainedLinearVelocities,
|
||||||
Vector3* constrainedAngularVelocities);
|
Vector3* constrainedAngularVelocities);
|
||||||
|
|
||||||
|
/// Set the constrained positions/orientations arrays
|
||||||
|
void setConstrainedPositionsArrays(Vector3* constrainedPositions,
|
||||||
|
Quaternion* constrainedOrientations);
|
||||||
};
|
};
|
||||||
|
|
||||||
// Set the constrained velocities arrays
|
// Set the constrained velocities arrays
|
||||||
|
@ -218,10 +204,17 @@ inline void ConstraintSolver::setConstrainedVelocitiesArrays(Vector3* constraine
|
||||||
Vector3* constrainedAngularVelocities) {
|
Vector3* constrainedAngularVelocities) {
|
||||||
assert(constrainedLinearVelocities != NULL);
|
assert(constrainedLinearVelocities != NULL);
|
||||||
assert(constrainedAngularVelocities != NULL);
|
assert(constrainedAngularVelocities != NULL);
|
||||||
mLinearVelocities = constrainedLinearVelocities;
|
mConstraintSolverData.linearVelocities = constrainedLinearVelocities;
|
||||||
mAngularVelocities = constrainedAngularVelocities;
|
mConstraintSolverData.angularVelocities = constrainedAngularVelocities;
|
||||||
mConstraintSolverData.linearVelocities = mLinearVelocities;
|
}
|
||||||
mConstraintSolverData.angularVelocities = mAngularVelocities;
|
|
||||||
|
// 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;
|
||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -46,9 +46,9 @@ using namespace std;
|
||||||
DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
|
DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_TIMESTEP)
|
||||||
: CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityEnabled(true),
|
: CollisionWorld(), mTimer(timeStep), mGravity(gravity), mIsGravityEnabled(true),
|
||||||
mConstrainedLinearVelocities(NULL), mConstrainedAngularVelocities(NULL),
|
mConstrainedLinearVelocities(NULL), mConstrainedAngularVelocities(NULL),
|
||||||
|
mConstrainedPositions(NULL), mConstrainedOrientations(NULL),
|
||||||
mContactSolver(mMapBodyToConstrainedVelocityIndex),
|
mContactSolver(mMapBodyToConstrainedVelocityIndex),
|
||||||
mConstraintSolver(mConstrainedPositions, mConstrainedOrientations,
|
mConstraintSolver(mMapBodyToConstrainedVelocityIndex),
|
||||||
mMapBodyToConstrainedVelocityIndex),
|
|
||||||
mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
|
mNbVelocitySolverIterations(DEFAULT_VELOCITY_SOLVER_NB_ITERATIONS),
|
||||||
mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
|
mNbPositionSolverIterations(DEFAULT_POSITION_SOLVER_NB_ITERATIONS),
|
||||||
mIsSleepingEnabled(SPLEEPING_ENABLED), mSplitLinearVelocities(NULL),
|
mIsSleepingEnabled(SPLEEPING_ENABLED), mSplitLinearVelocities(NULL),
|
||||||
|
@ -56,7 +56,7 @@ DynamicsWorld::DynamicsWorld(const Vector3 &gravity, decimal timeStep = DEFAULT_
|
||||||
mIslands(NULL), mNbBodiesCapacity(0),
|
mIslands(NULL), mNbBodiesCapacity(0),
|
||||||
mSleepLinearVelocity(DEFAULT_SLEEP_LINEAR_VELOCITY),
|
mSleepLinearVelocity(DEFAULT_SLEEP_LINEAR_VELOCITY),
|
||||||
mSleepAngularVelocity(DEFAULT_SLEEP_ANGULAR_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[] mSplitAngularVelocities;
|
||||||
delete[] mConstrainedLinearVelocities;
|
delete[] mConstrainedLinearVelocities;
|
||||||
delete[] mConstrainedAngularVelocities;
|
delete[] mConstrainedAngularVelocities;
|
||||||
|
delete[] mConstrainedPositions;
|
||||||
|
delete[] mConstrainedOrientations;
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef IS_PROFILING_ACTIVE
|
#ifdef IS_PROFILING_ACTIVE
|
||||||
|
@ -122,7 +124,7 @@ void DynamicsWorld::update() {
|
||||||
while(mTimer.isPossibleToTakeStep()) {
|
while(mTimer.isPossibleToTakeStep()) {
|
||||||
|
|
||||||
// Remove all contact manifolds
|
// Remove all contact manifolds
|
||||||
mContactManifolds.clear();
|
mCollisionDetection.mContactManifolds.clear();
|
||||||
|
|
||||||
// Reset all the contact manifolds lists of each body
|
// Reset all the contact manifolds lists of each body
|
||||||
resetContactManifoldListsOfBodies();
|
resetContactManifoldListsOfBodies();
|
||||||
|
@ -148,10 +150,10 @@ void DynamicsWorld::update() {
|
||||||
// Solve the position correction for constraints
|
// Solve the position correction for constraints
|
||||||
solvePositionCorrection();
|
solvePositionCorrection();
|
||||||
|
|
||||||
if (mIsSleepingEnabled) updateSleepingBodies();
|
// Update the state (positions and velocities) of the bodies
|
||||||
|
updateBodiesState();
|
||||||
|
|
||||||
// Update the AABBs of the bodies
|
if (mIsSleepingEnabled) updateSleepingBodies();
|
||||||
updateRigidBodiesAABB();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Reset the external force and torque applied to the bodies
|
// Reset the external force and torque applied to the bodies
|
||||||
|
@ -183,10 +185,6 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
|
||||||
Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
|
Vector3 newLinVelocity = mConstrainedLinearVelocities[indexArray];
|
||||||
Vector3 newAngVelocity = mConstrainedAngularVelocities[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
|
// Add the split impulse velocity from Contact Solver (only used
|
||||||
// to update the position)
|
// to update the position)
|
||||||
if (mContactSolver.isSplitImpulseActive()) {
|
if (mContactSolver.isSplitImpulseActive()) {
|
||||||
|
@ -199,38 +197,45 @@ void DynamicsWorld::integrateRigidBodiesPositions() {
|
||||||
const Vector3& currentPosition = bodies[b]->getTransform().getPosition();
|
const Vector3& currentPosition = bodies[b]->getTransform().getPosition();
|
||||||
const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation();
|
const Quaternion& currentOrientation = bodies[b]->getTransform().getOrientation();
|
||||||
|
|
||||||
// Compute the new position of the body
|
// Update the new constrained position and orientation of the body
|
||||||
Vector3 newPosition = currentPosition + newLinVelocity * dt;
|
mConstrainedPositions[indexArray] = currentPosition + newLinVelocity * dt;
|
||||||
Quaternion newOrientation = currentOrientation + Quaternion(0, newAngVelocity) *
|
mConstrainedOrientations[indexArray] = currentOrientation +
|
||||||
currentOrientation * decimal(0.5) * dt;
|
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 AABBs of the bodies
|
// Update the postion/orientation of the bodies
|
||||||
void DynamicsWorld::updateRigidBodiesAABB() {
|
void DynamicsWorld::updateBodiesState() {
|
||||||
|
|
||||||
PROFILE("DynamicsWorld::updateRigidBodiesAABB()");
|
PROFILE("DynamicsWorld::updateBodiesState()");
|
||||||
|
|
||||||
// For each rigid body of the world
|
// For each island of the world
|
||||||
set<RigidBody*>::iterator it;
|
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
|
||||||
for (it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
|
|
||||||
|
|
||||||
// If the body has moved
|
// For each body of the island
|
||||||
if ((*it)->mHasMoved) {
|
RigidBody** bodies = mIslands[islandIndex]->getBodies();
|
||||||
|
|
||||||
// Update the transform of the body due to the change of its center of mass
|
for (uint b=0; b < mIslands[islandIndex]->getNbBodies(); b++) {
|
||||||
(*it)->updateTransformWithCenterOfMass();
|
|
||||||
|
|
||||||
// Update the AABB of the rigid body
|
uint index = mMapBodyToConstrainedVelocityIndex.find(bodies[b])->second;
|
||||||
(*it)->updateAABB();
|
|
||||||
|
// 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;
|
delete[] mSplitAngularVelocities;
|
||||||
}
|
}
|
||||||
mNbBodiesCapacity = nbBodies;
|
mNbBodiesCapacity = nbBodies;
|
||||||
|
// TODO : Use better memory allocation here
|
||||||
mSplitLinearVelocities = new Vector3[mNbBodiesCapacity];
|
mSplitLinearVelocities = new Vector3[mNbBodiesCapacity];
|
||||||
mSplitAngularVelocities = new Vector3[mNbBodiesCapacity];
|
mSplitAngularVelocities = new Vector3[mNbBodiesCapacity];
|
||||||
mConstrainedLinearVelocities = new Vector3[mNbBodiesCapacity];
|
mConstrainedLinearVelocities = new Vector3[mNbBodiesCapacity];
|
||||||
mConstrainedAngularVelocities = new Vector3[mNbBodiesCapacity];
|
mConstrainedAngularVelocities = new Vector3[mNbBodiesCapacity];
|
||||||
|
mConstrainedPositions = new Vector3[mNbBodiesCapacity];
|
||||||
|
mConstrainedOrientations = new Quaternion[mNbBodiesCapacity];
|
||||||
assert(mSplitLinearVelocities != NULL);
|
assert(mSplitLinearVelocities != NULL);
|
||||||
assert(mSplitAngularVelocities != NULL);
|
assert(mSplitAngularVelocities != NULL);
|
||||||
assert(mConstrainedLinearVelocities != NULL);
|
assert(mConstrainedLinearVelocities != NULL);
|
||||||
assert(mConstrainedAngularVelocities != NULL);
|
assert(mConstrainedAngularVelocities != NULL);
|
||||||
|
assert(mConstrainedPositions != NULL);
|
||||||
|
assert(mConstrainedOrientations != NULL);
|
||||||
}
|
}
|
||||||
|
|
||||||
// Reset the velocities arrays
|
// Reset the velocities arrays
|
||||||
|
@ -381,6 +391,8 @@ void DynamicsWorld::solveContactsAndConstraints() {
|
||||||
mConstrainedAngularVelocities);
|
mConstrainedAngularVelocities);
|
||||||
mConstraintSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
|
mConstraintSolver.setConstrainedVelocitiesArrays(mConstrainedLinearVelocities,
|
||||||
mConstrainedAngularVelocities);
|
mConstrainedAngularVelocities);
|
||||||
|
mConstraintSolver.setConstrainedPositionsArrays(mConstrainedPositions,
|
||||||
|
mConstrainedOrientations);
|
||||||
|
|
||||||
// ---------- Solve velocity constraints for joints and contacts ---------- //
|
// ---------- Solve velocity constraints for joints and contacts ---------- //
|
||||||
|
|
||||||
|
@ -440,10 +452,6 @@ void DynamicsWorld::solvePositionCorrection() {
|
||||||
|
|
||||||
// ---------- Get the position/orientation of the rigid bodies ---------- //
|
// ---------- 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 each island of the world
|
||||||
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
|
for (uint islandIndex = 0; islandIndex < mNbIslands; islandIndex++) {
|
||||||
|
|
||||||
|
@ -467,27 +475,11 @@ void DynamicsWorld::solvePositionCorrection() {
|
||||||
// Solve the position constraints
|
// Solve the position constraints
|
||||||
mConstraintSolver.solvePositionConstraints(mIslands[islandIndex]);
|
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
|
// 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
|
// Compute the body ID
|
||||||
bodyindex bodyID = computeNextAvailableBodyID();
|
bodyindex bodyID = computeNextAvailableBodyID();
|
||||||
|
@ -497,17 +489,13 @@ RigidBody* DynamicsWorld::createRigidBody(const Transform& transform, decimal ma
|
||||||
|
|
||||||
// Create the rigid body
|
// Create the rigid body
|
||||||
RigidBody* rigidBody = new (mMemoryAllocator.allocate(sizeof(RigidBody))) RigidBody(transform,
|
RigidBody* rigidBody = new (mMemoryAllocator.allocate(sizeof(RigidBody))) RigidBody(transform,
|
||||||
mass, bodyID);
|
*this, bodyID);
|
||||||
assert(rigidBody != NULL);
|
assert(rigidBody != NULL);
|
||||||
|
|
||||||
// Add the rigid body to the physics world
|
// Add the rigid body to the physics world
|
||||||
mBodies.insert(rigidBody);
|
mBodies.insert(rigidBody);
|
||||||
mRigidBodies.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 the pointer to the rigid body
|
||||||
return rigidBody;
|
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
|
// Destroy a rigid body and all the joints which it belongs
|
||||||
void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
|
void DynamicsWorld::destroyRigidBody(RigidBody* rigidBody) {
|
||||||
|
|
||||||
// TODO : DELETE THIS
|
// Remove all the collision shapes of the body
|
||||||
// Remove the body from the collision detection
|
rigidBody->removeAllCollisionShapes();
|
||||||
//mCollisionDetection.removeProxyCollisionShape(rigidBody);
|
|
||||||
|
|
||||||
// Add the body ID to the list of free IDs
|
// Add the body ID to the list of free IDs
|
||||||
mFreeBodiesIDs.push_back(rigidBody->getID());
|
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
|
// Destroy all the joints in which the rigid body to be destroyed is involved
|
||||||
JointListElement* element;
|
JointListElement* element;
|
||||||
for (element = rigidBody->mJointsList; element != NULL; element = element->next) {
|
for (element = rigidBody->mJointsList; element != NULL; element = element->next) {
|
||||||
|
@ -665,29 +648,6 @@ void DynamicsWorld::addJointToBody(Joint* joint) {
|
||||||
joint->mBody2->mJointsList = jointListElement2;
|
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
|
// Reset all the contact manifolds linked list of each body
|
||||||
void DynamicsWorld::resetContactManifoldListsOfBodies() {
|
void DynamicsWorld::resetContactManifoldListsOfBodies() {
|
||||||
|
|
||||||
|
@ -736,8 +696,8 @@ void DynamicsWorld::computeIslands() {
|
||||||
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
|
for (std::set<RigidBody*>::iterator it = mRigidBodies.begin(); it != mRigidBodies.end(); ++it) {
|
||||||
(*it)->mIsAlreadyInIsland = false;
|
(*it)->mIsAlreadyInIsland = false;
|
||||||
}
|
}
|
||||||
for (std::vector<ContactManifold*>::iterator it = mContactManifolds.begin();
|
for (std::vector<ContactManifold*>::iterator it = mCollisionDetection.mContactManifolds.begin();
|
||||||
it != mContactManifolds.end(); ++it) {
|
it != mCollisionDetection.mContactManifolds.end(); ++it) {
|
||||||
(*it)->mIsAlreadyInIsland = false;
|
(*it)->mIsAlreadyInIsland = false;
|
||||||
}
|
}
|
||||||
for (std::set<Joint*>::iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
|
for (std::set<Joint*>::iterator it = mJoints.begin(); it != mJoints.end(); ++it) {
|
||||||
|
@ -770,7 +730,8 @@ void DynamicsWorld::computeIslands() {
|
||||||
|
|
||||||
// Create the new island
|
// Create the new island
|
||||||
void* allocatedMemoryIsland = mMemoryAllocator.allocate(sizeof(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);
|
mJoints.size(), mMemoryAllocator);
|
||||||
|
|
||||||
// While there are still some bodies to visit in the stack
|
// 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
|
// Enable/Disable the sleeping technique
|
||||||
void DynamicsWorld::enableSleeping(bool isSleepingEnabled) {
|
void DynamicsWorld::enableSleeping(bool isSleepingEnabled) {
|
||||||
mIsSleepingEnabled = isSleepingEnabled;
|
mIsSleepingEnabled = isSleepingEnabled;
|
||||||
|
|
|
@ -72,10 +72,6 @@ class DynamicsWorld : public CollisionWorld {
|
||||||
/// All the rigid bodies of the physics world
|
/// All the rigid bodies of the physics world
|
||||||
std::set<RigidBody*> mRigidBodies;
|
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
|
/// All the joints of the world
|
||||||
std::set<Joint*> mJoints;
|
std::set<Joint*> mJoints;
|
||||||
|
|
||||||
|
@ -100,10 +96,10 @@ class DynamicsWorld : public CollisionWorld {
|
||||||
Vector3* mSplitAngularVelocities;
|
Vector3* mSplitAngularVelocities;
|
||||||
|
|
||||||
/// Array of constrained rigid bodies position (for position error correction)
|
/// 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)
|
/// 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
|
/// Map body to their index in the constrained velocities array
|
||||||
std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex;
|
std::map<RigidBody*, uint> mMapBodyToConstrainedVelocityIndex;
|
||||||
|
@ -130,9 +126,6 @@ class DynamicsWorld : public CollisionWorld {
|
||||||
/// becomes smaller than the sleep velocity.
|
/// becomes smaller than the sleep velocity.
|
||||||
decimal mTimeBeforeSleep;
|
decimal mTimeBeforeSleep;
|
||||||
|
|
||||||
/// Pointer to an event listener object
|
|
||||||
EventListener* mEventListener;
|
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
|
||||||
/// Private copy-constructor
|
/// Private copy-constructor
|
||||||
|
@ -154,11 +147,6 @@ class DynamicsWorld : public CollisionWorld {
|
||||||
void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
|
void updatePositionAndOrientationOfBody(RigidBody* body, Vector3 newLinVelocity,
|
||||||
Vector3 newAngVelocity);
|
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
|
/// Compute and set the interpolation factor to all bodies
|
||||||
void setInterpolationFactorToAllBodies();
|
void setInterpolationFactorToAllBodies();
|
||||||
|
|
||||||
|
@ -180,16 +168,12 @@ class DynamicsWorld : public CollisionWorld {
|
||||||
/// Compute the islands of awake bodies.
|
/// Compute the islands of awake bodies.
|
||||||
void computeIslands();
|
void computeIslands();
|
||||||
|
|
||||||
|
/// Update the postion/orientation of the bodies
|
||||||
|
void updateBodiesState();
|
||||||
|
|
||||||
/// Put bodies to sleep if needed.
|
/// Put bodies to sleep if needed.
|
||||||
void updateSleepingBodies();
|
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 :
|
public :
|
||||||
|
|
||||||
// -------------------- Methods -------------------- //
|
// -------------------- Methods -------------------- //
|
||||||
|
@ -226,7 +210,7 @@ class DynamicsWorld : public CollisionWorld {
|
||||||
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
|
void setIsSolveFrictionAtContactManifoldCenterActive(bool isActive);
|
||||||
|
|
||||||
/// Create a rigid body into the physics world.
|
/// 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
|
/// Destroy a rigid body and all the joints which it belongs
|
||||||
void destroyRigidBody(RigidBody* rigidBody);
|
void destroyRigidBody(RigidBody* rigidBody);
|
||||||
|
@ -359,21 +343,6 @@ inline void DynamicsWorld::setIsSolveFrictionAtContactManifoldCenterActive(bool
|
||||||
mContactSolver.setIsSolveFrictionAtContactManifoldCenterActive(isActive);
|
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
|
// Return the gravity vector of the world
|
||||||
inline Vector3 DynamicsWorld::getGravity() const {
|
inline Vector3 DynamicsWorld::getGravity() const {
|
||||||
return mGravity;
|
return mGravity;
|
||||||
|
@ -410,13 +379,15 @@ inline std::set<RigidBody*>::iterator DynamicsWorld::getRigidBodiesEndIterator()
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return a reference to the contact manifolds of the world
|
// 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 {
|
inline const std::vector<ContactManifold*>& DynamicsWorld::getContactManifolds() const {
|
||||||
return mContactManifolds;
|
return mCollisionDetection.mContactManifolds;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the number of contact manifolds in the world
|
// 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 {
|
inline uint DynamicsWorld::getNbContactManifolds() const {
|
||||||
return mContactManifolds.size();
|
return mCollisionDetection.mContactManifolds.size();
|
||||||
}
|
}
|
||||||
|
|
||||||
// Return the current physics time (in seconds)
|
// Return the current physics time (in seconds)
|
||||||
|
|
|
@ -30,9 +30,10 @@ using namespace reactphysics3d;
|
||||||
|
|
||||||
|
|
||||||
// Constructor
|
// Constructor
|
||||||
OverlappingPair::OverlappingPair(CollisionBody* body1, CollisionBody* body2,
|
OverlappingPair::OverlappingPair(ProxyShape* shape1, ProxyShape* shape2,
|
||||||
MemoryAllocator &memoryAllocator)
|
MemoryAllocator& memoryAllocator)
|
||||||
: mBody1(body1), mBody2(body2), mContactManifold(body1, body2, memoryAllocator),
|
: mShape1(shape1), mShape2(shape2),
|
||||||
|
mContactManifold(shape1->getBody(), shape2->getBody(), memoryAllocator),
|
||||||
mCachedSeparatingAxis(1.0, 1.0, 1.0) {
|
mCachedSeparatingAxis(1.0, 1.0, 1.0) {
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
|
@ -133,7 +133,8 @@ inline void OverlappingPair::addContact(ContactPoint* contact) {
|
||||||
|
|
||||||
// Update the contact manifold
|
// Update the contact manifold
|
||||||
inline void OverlappingPair::update() {
|
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
|
// Return the cached separating axis
|
||||||
|
|
Loading…
Reference in New Issue
Block a user