From a983026094b694408adda8fdb2bb6781286e1b86 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Thu, 3 Jul 2014 00:13:30 +0200 Subject: [PATCH] Inflate the AABB in direction of linear motion in Dynamic AABB tree --- src/body/CollisionBody.h | 2 +- src/body/RigidBody.cpp | 20 ++++++++++++- src/body/RigidBody.h | 5 +++- src/collision/CollisionDetection.h | 8 +++-- .../broadphase/BroadPhaseAlgorithm.cpp | 5 ++-- .../broadphase/BroadPhaseAlgorithm.h | 3 +- src/collision/broadphase/DynamicAABBTree.cpp | 30 +++++++++++++++++-- src/collision/broadphase/DynamicAABBTree.h | 2 +- src/configuration.h | 10 +++++-- src/engine/CollisionWorld.h | 2 +- src/engine/DynamicsWorld.h | 4 +++ 11 files changed, 75 insertions(+), 16 deletions(-) diff --git a/src/body/CollisionBody.h b/src/body/CollisionBody.h index 08b764f5..ec091f32 100644 --- a/src/body/CollisionBody.h +++ b/src/body/CollisionBody.h @@ -111,7 +111,7 @@ class CollisionBody : public Body { void updateOldTransform(); /// Update the broad-phase state for this body (because it has moved for instance) - void updateBroadPhaseState() const; + virtual void updateBroadPhaseState() const; /// Ask the broad-phase to test again the collision shapes of the body for collision /// (as if the body has moved). diff --git a/src/body/RigidBody.cpp b/src/body/RigidBody.cpp index ddad4bef..8ba46008 100644 --- a/src/body/RigidBody.cpp +++ b/src/body/RigidBody.cpp @@ -27,7 +27,7 @@ #include "RigidBody.h" #include "constraint/Joint.h" #include "../collision/shapes/CollisionShape.h" -#include "../engine/CollisionWorld.h" +#include "../engine/DynamicsWorld.h" // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; @@ -295,3 +295,21 @@ void RigidBody::recomputeMassInformation() { mLinearVelocity += mAngularVelocity.cross(mCenterOfMassWorld - oldCenterOfMass); } +// Update the broad-phase state for this body (because it has moved for instance) +void RigidBody::updateBroadPhaseState() const { + + DynamicsWorld& world = dynamic_cast(mWorld); + const Vector3 displacement = world.mTimer.getTimeStep() * mLinearVelocity; + + // For all the proxy collision shapes of the body + for (ProxyShape* shape = mProxyCollisionShapes; shape != NULL; shape = shape->mNext) { + + // Recompute the world-space AABB of the collision shape + AABB aabb; + shape->getCollisionShape()->computeAABB(aabb, mTransform *shape->getLocalToBodyTransform()); + + // Update the broad-phase state for the proxy collision shape + mWorld.mCollisionDetection.updateProxyCollisionShape(shape, aabb, displacement); + } +} + diff --git a/src/body/RigidBody.h b/src/body/RigidBody.h index 38d60b5d..3111733f 100644 --- a/src/body/RigidBody.h +++ b/src/body/RigidBody.h @@ -39,7 +39,7 @@ namespace reactphysics3d { // Class declarations struct JointListElement; class Joint; - +class DynamicsWorld; // Class RigidBody /** @@ -115,6 +115,9 @@ class RigidBody : public CollisionBody { /// Update the transform of the body after a change of the center of mass void updateTransformWithCenterOfMass(); + /// Update the broad-phase state for this body (because it has moved for instance) + virtual void updateBroadPhaseState() const; + public : // -------------------- Methods -------------------- // diff --git a/src/collision/CollisionDetection.h b/src/collision/CollisionDetection.h index bb0e1d49..b405ec12 100644 --- a/src/collision/CollisionDetection.h +++ b/src/collision/CollisionDetection.h @@ -127,7 +127,8 @@ class CollisionDetection { void removeProxyCollisionShape(ProxyShape* proxyShape); /// Update a proxy collision shape (that has moved for instance) - void updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb); + void updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb, + const Vector3& displacement = Vector3(0, 0, 0)); /// Add a pair of bodies that cannot collide with each other void addNoCollisionPair(CollisionBody* body1, CollisionBody* body2); @@ -194,8 +195,9 @@ inline void CollisionDetection::askForBroadPhaseCollisionCheck(ProxyShape* shape } // Update a proxy collision shape (that has moved for instance) -inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb) { - mBroadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb); +inline void CollisionDetection::updateProxyCollisionShape(ProxyShape* shape, const AABB& aabb, + const Vector3& displacement) { + mBroadPhaseAlgorithm.updateProxyCollisionShape(shape, aabb, displacement); } } diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.cpp b/src/collision/broadphase/BroadPhaseAlgorithm.cpp index f6e88301..41649de1 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.cpp +++ b/src/collision/broadphase/BroadPhaseAlgorithm.cpp @@ -138,14 +138,15 @@ void BroadPhaseAlgorithm::removeProxyCollisionShape(ProxyShape* proxyShape) { } // Notify the broad-phase that a collision shape has moved and need to be updated -void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb) { +void BroadPhaseAlgorithm::updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb, + const Vector3& displacement) { int broadPhaseID = proxyShape->mBroadPhaseID; assert(broadPhaseID >= 0); // Update the dynamic AABB tree according to the movement of the collision shape - bool hasBeenReInserted = mDynamicAABBTree.updateObject(broadPhaseID, aabb); + bool hasBeenReInserted = mDynamicAABBTree.updateObject(broadPhaseID, aabb, displacement); // If the collision shape has moved out of its fat AABB (and therefore has been reinserted // into the tree). diff --git a/src/collision/broadphase/BroadPhaseAlgorithm.h b/src/collision/broadphase/BroadPhaseAlgorithm.h index c6cecb6b..0a27fd32 100644 --- a/src/collision/broadphase/BroadPhaseAlgorithm.h +++ b/src/collision/broadphase/BroadPhaseAlgorithm.h @@ -137,7 +137,8 @@ class BroadPhaseAlgorithm { void removeProxyCollisionShape(ProxyShape* proxyShape); /// Notify the broad-phase that a collision shape has moved and need to be updated - void updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb); + void updateProxyCollisionShape(ProxyShape* proxyShape, const AABB& aabb, + const Vector3& displacement); /// Add a collision shape in the array of shapes that have moved in the last simulation step /// and that need to be tested again for broad-phase overlapping. diff --git a/src/collision/broadphase/DynamicAABBTree.cpp b/src/collision/broadphase/DynamicAABBTree.cpp index 305aeadf..89c2705d 100644 --- a/src/collision/broadphase/DynamicAABBTree.cpp +++ b/src/collision/broadphase/DynamicAABBTree.cpp @@ -154,8 +154,10 @@ void DynamicAABBTree::removeObject(int nodeID) { // Update the dynamic tree after an object has moved. /// If the new AABB of the object that has moved is still inside its fat AABB, then /// nothing is done. Otherwise, the corresponding node is removed and reinserted into the tree. -/// The method returns true if the object has been reinserted into the tree. -bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB) { +/// The method returns true if the object has been reinserted into the tree. The "displacement" +/// argument is the linear velocity of the AABB multiplied by the elapsed time between two +/// frames. +bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement) { assert(nodeID >= 0 && nodeID < mNbAllocatedNodes); assert(mNodes[nodeID].isLeaf()); @@ -169,12 +171,34 @@ bool DynamicAABBTree::updateObject(int nodeID, const AABB& newAABB) { // If the new AABB is outside the fat AABB, we remove the corresponding node removeLeafNode(nodeID); - // Compute a new fat AABB for the new AABB + // Compute the fat AABB by inflating the AABB with a constant gap mNodes[nodeID].aabb = newAABB; const Vector3 gap(DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP, DYNAMIC_TREE_AABB_GAP); mNodes[nodeID].aabb.mMinCoordinates -= gap; mNodes[nodeID].aabb.mMaxCoordinates += gap; + // Inflate the fat AABB in direction of the linear motion of the AABB + if (displacement.x < decimal(0.0)) { + mNodes[nodeID].aabb.mMinCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x; + } + else { + mNodes[nodeID].aabb.mMaxCoordinates.x += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.x; + } + if (displacement.y < decimal(0.0)) { + mNodes[nodeID].aabb.mMinCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y; + } + else { + mNodes[nodeID].aabb.mMaxCoordinates.y += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.y; + } + if (displacement.z < decimal(0.0)) { + mNodes[nodeID].aabb.mMinCoordinates.z += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z; + } + else { + mNodes[nodeID].aabb.mMaxCoordinates.z += DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER *displacement.z; + } + + assert(mNodes[nodeID].aabb.contains(newAABB)); + // Reinsert the node into the tree insertLeafNode(nodeID); diff --git a/src/collision/broadphase/DynamicAABBTree.h b/src/collision/broadphase/DynamicAABBTree.h index 207ebf8a..ef6686f9 100644 --- a/src/collision/broadphase/DynamicAABBTree.h +++ b/src/collision/broadphase/DynamicAABBTree.h @@ -146,7 +146,7 @@ class DynamicAABBTree { void removeObject(int nodeID); /// Update the dynamic tree after an object has moved. - bool updateObject(int nodeID, const AABB& newAABB); + bool updateObject(int nodeID, const AABB& newAABB, const Vector3& displacement); /// Return the fat AABB corresponding to a given node ID const AABB& getFatAABB(int nodeID) const; diff --git a/src/configuration.h b/src/configuration.h index 2e011cb1..6cb26b4c 100644 --- a/src/configuration.h +++ b/src/configuration.h @@ -122,9 +122,15 @@ const decimal DEFAULT_SLEEP_LINEAR_VELOCITY = decimal(0.02); const decimal DEFAULT_SLEEP_ANGULAR_VELOCITY = decimal(3.0 * (PI / 180.0)); /// In the broad-phase collision detection (dynamic AABB tree), the AABBs are -/// fatten to allow the collision shape to move a little bit without triggering -/// a large modification of the tree which can be costly +/// inflated with a constant gap to allow the collision shape to move a little bit +/// without triggering a large modification of the tree which can be costly const decimal DYNAMIC_TREE_AABB_GAP = decimal(0.1); + +/// In the broad-phase collision detection (dynamic AABB tree), the AABBs are +/// also inflated in direction of the linear motion of the body by mutliplying the +/// followin constant with the linear velocity and the elapsed time between two frames. +const decimal DYNAMIC_TREE_AABB_LIN_GAP_MULTIPLIER = decimal(2.0); + } #endif diff --git a/src/engine/CollisionWorld.h b/src/engine/CollisionWorld.h index c427d3d1..68f60f61 100644 --- a/src/engine/CollisionWorld.h +++ b/src/engine/CollisionWorld.h @@ -119,7 +119,7 @@ class CollisionWorld { /// Destroy a collision body void destroyCollisionBody(CollisionBody* collisionBody); - // -------------------- Friends -------------------- // + // -------------------- Friendship -------------------- // friend class CollisionDetection; friend class CollisionBody; diff --git a/src/engine/DynamicsWorld.h b/src/engine/DynamicsWorld.h index 928eb111..70ef5522 100644 --- a/src/engine/DynamicsWorld.h +++ b/src/engine/DynamicsWorld.h @@ -283,6 +283,10 @@ class DynamicsWorld : public CollisionWorld { /// Set an event listener object to receive events callbacks. void setEventListener(EventListener* eventListener); + + // -------------------- Friendship -------------------- // + + friend class RigidBody; }; // Reset the external force and torque applied to the bodies