Inflate the AABB in direction of linear motion in Dynamic AABB tree

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).

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<DynamicsWorld&>(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);
+    }
+}
+

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 -------------------- //

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);
 }
 
 }

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).

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.

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);
 

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;

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

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;

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