Do not compute the AABB of a sphere by rotating its extents

src/body/CollisionBody.cpp

@@ -44,13 +44,11 @@ CollisionBody::CollisionBody(const Transform& transform, CollisionShape *collisi
     // Initialize the old transform
     mOldTransform = transform;
 
-    // Create the AABB for broad-phase collision detection
-    mAabb = new AABB(transform, collisionShape->getLocalExtents(OBJECT_MARGIN));
+    // Initialize the AABB for broad-phase collision detection
+    mCollisionShape->updateAABB(mAabb, transform);
 }
 
 // Destructor
 CollisionBody::~CollisionBody() {
 
-    // Delete the AABB
-    delete mAabb;
 }

src/body/CollisionBody.h

@@ -71,7 +71,7 @@ class CollisionBody : public Body {
         bool mIsCollisionEnabled;
 
         /// AABB for Broad-Phase collision detection
-        AABB* mAabb;
+        AABB mAabb;
 
         /// True if the body has moved during the last frame
         bool mHasMoved;
@@ -119,7 +119,7 @@ class CollisionBody : public Body {
         void setTransform(const Transform& transform);
 
         /// Return the AAABB of the body
-        const AABB* getAABB() const;
+        const AABB& getAABB() const;
 
         /// Return the interpolated transform for rendering
         Transform getInterpolatedTransform() const;
@@ -216,7 +216,7 @@ inline void CollisionBody::setTransform(const Transform& transform) {
 }
 
 // Return the AAABB of the body
-inline const AABB* CollisionBody::getAABB() const {
+inline const AABB& CollisionBody::getAABB() const {
     return mAabb;
 }
 
@@ -242,7 +242,7 @@ inline void CollisionBody::updateAABB() {
     // TODO : An AABB should not be updated every frame but only if the body has moved
 
     // Update the AABB
-    mAabb->update(mTransform, mCollisionShape->getLocalExtents(OBJECT_MARGIN));
+    mCollisionShape->updateAABB(mAabb, mTransform);
 }
 
 }

src/body/RigidBody.cpp

@@ -39,11 +39,7 @@ using namespace reactphysics3d;
 
     mRestitution = decimal(1.0);
 
-    // Set the body pointer of the AABB and the collision shape
-    mAabb->setBodyPointer(this);
-
     assert(collisionShape);
-    assert(mAabb);
 }
 
 // Destructor

src/collision/CollisionDetection.cpp

@@ -37,8 +37,6 @@
 #include <set>
 #include <utility>
 #include <utility>
-#include <sys/time.h>       // TODO : Delete this
-#include <iostream>         // TODO : Delete this
 
 // We want to use the ReactPhysics3D namespace
 using namespace reactphysics3d;
@@ -62,28 +60,13 @@ CollisionDetection::~CollisionDetection() {
 }
 
 // Compute the collision detection
-bool CollisionDetection::computeCollisionDetection() {
+void CollisionDetection::computeCollisionDetection() {
 	    
-    // TODO : Remove this code
-    timeval timeValueStart;
-    timeval timeValueStop;
-    gettimeofday(&timeValueStart, NULL);
-
     // Compute the broad-phase collision detection
     computeBroadPhase();
     
-    // TODO : Remove this code
-    gettimeofday(&timeValueStop, NULL);
-    double startTime = timeValueStart.tv_sec * 1000000.0 + (timeValueStart.tv_usec);
-    double stopTime = timeValueStop.tv_sec * 1000000.0 + (timeValueStop.tv_usec);
-    double deltaTime = stopTime - startTime;
-    //printf("Broadphase time : %f micro sec \n", deltaTime);
-    
     // Compute the narrow-phase collision detection
-    bool collisionExists = computeNarrowPhase();
-	
-    // Return true if at least one contact has been found
-    return collisionExists;
+    computeNarrowPhase();
 }
 
 // Compute the broad-phase collision detection
@@ -96,14 +79,13 @@ void CollisionDetection::computeBroadPhase() {
         if ((*it)->getHasMoved()) {
 
             // Notify the broad-phase that the body has moved
-            mBroadPhaseAlgorithm->updateObject(*it, *((*it)->getAABB()));
+            mBroadPhaseAlgorithm->updateObject(*it, (*it)->getAABB());
         }
     }  
 }
 
 // Compute the narrow-phase collision detection
-bool CollisionDetection::computeNarrowPhase() {
-    bool collisionExists = false;
+void CollisionDetection::computeNarrowPhase() {
     map<bodyindexpair, BroadPhasePair*>::iterator it;
     
     // For each possible collision pair of bodies
@@ -129,7 +111,6 @@ bool CollisionDetection::computeNarrowPhase() {
         if (narrowPhaseAlgorithm.testCollision(body1->getCollisionShape(), body1->getTransform(),
                                                body2->getCollisionShape(), body2->getTransform(), contactInfo)) {
             assert(contactInfo != NULL);
-            collisionExists = true;
 
             // Notify the world about the new narrow-phase contact
             mWorld->notifyNewContact(pair, contactInfo);
@@ -139,8 +120,6 @@ bool CollisionDetection::computeNarrowPhase() {
             mMemoryPoolContactInfos.freeObject(contactInfo);
         }
     }
-    
-    return collisionExists;
 }
 
 // Allow the broadphase to notify the collision detection about an overlapping pair.

src/collision/CollisionDetection.h

@@ -94,7 +94,7 @@ class CollisionDetection {
         void computeBroadPhase();
 
         /// Compute the narrow-phase collision detection
-        bool computeNarrowPhase();
+        void computeNarrowPhase();
 
         /// Select the narrow phase algorithm to use given two collision shapes
         NarrowPhaseAlgorithm& SelectNarrowPhaseAlgorithm(CollisionShape* collisionShape1,
@@ -117,7 +117,7 @@ class CollisionDetection {
         void removeBody(CollisionBody* body);
 
         /// Compute the collision detection
-        bool computeCollisionDetection();
+        void computeCollisionDetection();
 
         /// Allow the broadphase to notify the collision detection about a new overlapping pair.
         void broadPhaseNotifyAddedOverlappingPair(BodyPair* pair);
@@ -143,7 +143,7 @@ inline NarrowPhaseAlgorithm& CollisionDetection::SelectNarrowPhaseAlgorithm(
 inline void CollisionDetection::addBody(CollisionBody* body) {
     
     // Add the body to the broad-phase
-    mBroadPhaseAlgorithm->addObject(body, *(body->getAABB()));
+    mBroadPhaseAlgorithm->addObject(body, body->getAABB());
 }  
 
 // Remove a body from the collision detection

src/collision/broadphase/SweepAndPruneAlgorithm.cpp

@@ -131,7 +131,7 @@ void SweepAndPruneAlgorithm::removeObject(CollisionBody* body) {
     // in order to remove all overlapping pairs from the pair manager
     const decimal max = DECIMAL_LARGEST;
     const Vector3 maxVector(max, max, max);
-    const AABB aabb(maxVector, maxVector, body);
+    const AABB aabb(maxVector, maxVector);
     updateObject(body, aabb);
 
     // Get the corresponding box

src/collision/shapes/AABB.cpp

@@ -45,21 +45,16 @@ using namespace reactphysics3d;
 using namespace std;
 
 // Constructor
-AABB::AABB() : mBodyPointer(NULL) {
+AABB::AABB() {
     
 }
 
 // Constructor
-AABB::AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates, Body* modyPointer)
-     :mMinCoordinates(minCoordinates), mMaxCoordinates(maxCoordinates), mBodyPointer(modyPointer) {
+AABB::AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates)
+     :mMinCoordinates(minCoordinates), mMaxCoordinates(maxCoordinates) {
 
 }
 
-// Constructor
-AABB::AABB(const Transform& transform, const Vector3& extents) : mBodyPointer(NULL) {
-    update(transform, extents);
-}
-
 // Destructor
 AABB::~AABB() {
 

src/collision/shapes/AABB.h

@@ -54,10 +54,6 @@ class AABB {
         /// Maximum world coordinates of the AABB on the x,y and z axis
         Vector3 mMaxCoordinates;
 
-        /// Pointer to the owner body (not the abstract class Body
-        /// but its derivative which is instanciable)
-        Body* mBodyPointer;
-
         // -------------------- Methods -------------------- //
 
         /// Private copy-constructor
@@ -66,6 +62,9 @@ class AABB {
         /// Private assignment operator
         AABB& operator=(const AABB& aabb);
 
+        /// Constructor
+        AABB(const Transform& transform, const Vector3& extents);
+
     public :
 
         // -------------------- Methods -------------------- //
@@ -74,10 +73,9 @@ class AABB {
         AABB();
 
         /// Constructor
-        AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates, Body* modyPointer);
+        AABB(const Vector3& minCoordinates, const Vector3& maxCoordinates);
+
 
-        /// Constructor
-        AABB(const Transform& transform, const Vector3& extents);
 
         /// Destructor
         virtual ~AABB();
@@ -88,22 +86,18 @@ class AABB {
         /// Return the minimum coordinates of the AABB
         const Vector3& getMin() const;
 
+        /// Set the minimum coordinates of the AABB
+        void setMin(const Vector3& min);
+
         /// Return the maximum coordinates of the AABB
         const Vector3& getMax() const;
 
-        /// Return a pointer to the owner body
-        Body* getBodyPointer() const;
-
-        /// Set the body pointer
-        void setBodyPointer(Body* bodyPointer);
+        /// Set the maximum coordinates of the AABB
+        void setMax(const Vector3& max);
 
         /// Return true if the current AABB is overlapping with the AABB in argument
         bool testCollision(const AABB& aabb) const;
 
-        /// Update the oriented bounding box orientation
-        /// according to a new orientation of the rigid body
-        virtual void update(const Transform& newTransform, const Vector3& extents);
-
 #ifdef VISUAL_DEBUG
        /// Draw the AABB (only for testing purpose)
        virtual void draw() const;
@@ -120,19 +114,19 @@ inline const Vector3& AABB::getMin() const {
     return mMinCoordinates;
 }
 
+// Set the minimum coordinates of the AABB
+inline void AABB::setMin(const Vector3& min) {
+    mMinCoordinates = min;
+}
+
 // Return the maximum coordinates of the AABB
 inline const Vector3& AABB::getMax() const {
     return mMaxCoordinates;
 }
 
-// Return a pointer to the owner body
-inline Body* AABB::getBodyPointer() const {
-    return mBodyPointer;
-}
-
-// Set the body pointer
-inline void AABB::setBodyPointer(Body* bodyPointer) {
-    mBodyPointer = bodyPointer;
+/// Set the maximum coordinates of the AABB
+inline void AABB::setMax(const Vector3& max) {
+    mMaxCoordinates = max;
 }
 
 // Return true if the current AABB is overlapping with the AABB in argument.
@@ -147,16 +141,6 @@ inline bool AABB::testCollision(const AABB& aabb) const {
     return true;
 }
 
-// Update the world minimum and maximum coordinates of the AABB on the three x,y and z axis
-inline void AABB::update(const Transform& newTransform, const Vector3& extents) {
-    Matrix3x3 worldAxis = newTransform.getOrientation().getMatrix().getAbsoluteMatrix();
-    Vector3 worldExtents = Vector3(worldAxis.getColumn(0).dot(extents),
-                                     worldAxis.getColumn(1).dot(extents),
-                                     worldAxis.getColumn(2).dot(extents));
-    mMinCoordinates = newTransform.getPosition() - worldExtents;
-    mMaxCoordinates = newTransform.getPosition() + worldExtents;
-}
-
 }
 
 #endif

src/collision/shapes/CollisionShape.cpp

@@ -38,3 +38,24 @@ CollisionShape::CollisionShape(CollisionShapeType type) : mType(type) {
 CollisionShape::~CollisionShape() {
 
 }
+
+// Update the AABB of a body using its collision shape
+inline void CollisionShape::updateAABB(AABB& aabb, const Transform& transform) {
+
+    // Get the local extents in x,y and z direction
+    Vector3 extents = getLocalExtents(OBJECT_MARGIN);
+
+    // Rotate the local extents according to the orientation of the body
+    Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsoluteMatrix();
+    Vector3 worldExtents = Vector3(worldAxis.getColumn(0).dot(extents),
+                                   worldAxis.getColumn(1).dot(extents),
+                                   worldAxis.getColumn(2).dot(extents));
+
+    // Compute the minimum and maximum coordinates of the rotated extents
+    Vector3 minCoordinates = transform.getPosition() - worldExtents;
+    Vector3 maxCoordinates = transform.getPosition() + worldExtents;
+
+    // Update the AABB with the new minimum and maximum coordinates
+    aabb.setMin(minCoordinates);
+    aabb.setMax(maxCoordinates);
+}

src/collision/shapes/CollisionShape.h

@@ -30,6 +30,7 @@
 #include <cassert>
 #include "../../mathematics/Vector3.h"
 #include "../../mathematics/Matrix3x3.h"
+#include "AABB.h"
 
 /// ReactPhysics3D namespace
 namespace reactphysics3d {
@@ -89,12 +90,15 @@ class CollisionShape {
 
         /// Return the local inertia tensor of the collision shapes
         virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0;
+
+        /// Update the AABB of a body using its collision shape
+        virtual void updateAABB(AABB& aabb, const Transform& transform);
 };
 
 // Return the type of the collision shape
 inline CollisionShapeType CollisionShape::getType() const {
     return mType;
-}                                                                  
+}
 
 }
 

src/collision/shapes/SphereShape.h

@@ -86,6 +86,9 @@ class SphereShape : public CollisionShape {
         /// Return the margin distance around the shape
         virtual decimal getMargin() const;
 
+        /// Update the AABB of a body using its collision shape
+        virtual void updateAABB(AABB& aabb, const Transform& transform);
+
 #ifdef VISUAL_DEBUG
         /// Draw the sphere (only for testing purpose)
         virtual void draw() const;
@@ -145,6 +148,21 @@ inline decimal SphereShape::getMargin() const {
     return mRadius + OBJECT_MARGIN;
 }
 
+// Update the AABB of a body using its collision shape
+inline void SphereShape::updateAABB(AABB& aabb, const Transform& transform) {
+
+    // Get the local extents in x,y and z direction
+    Vector3 extents = getLocalExtents(OBJECT_MARGIN);
+
+    // Compute the minimum and maximum coordinates of the rotated extents
+    Vector3 minCoordinates = transform.getPosition() - extents;
+    Vector3 maxCoordinates = transform.getPosition() + extents;
+
+    // Update the AABB with the new minimum and maximum coordinates
+    aabb.setMin(minCoordinates);
+    aabb.setMax(maxCoordinates);
+}
+
 }
 
 #endif

src/engine/DynamicsWorld.cpp

@@ -289,7 +289,7 @@ void DynamicsWorld::removeAllConstraints() {
 void DynamicsWorld::notifyAddedOverlappingPair(const BroadPhasePair* addedPair) {
 
     // Get the pair of body index
-    std::pair<bodyindex, bodyindex> indexPair = addedPair->getBodiesIndexPair();
+    bodyindexpair indexPair = addedPair->getBodiesIndexPair();
 
     // Add the pair into the set of overlapping pairs (if not there yet)
     OverlappingPair* newPair = new (mMemoryPoolOverlappingPairs.allocateObject()) OverlappingPair(