From 6e322882eb21e66a44a1f055beb75f4b52f9408e Mon Sep 17 00:00:00 2001
From: Daniel Chappuis <chappuis.daniel@gmail.com>
Date: Sun, 5 Nov 2017 23:10:55 +0100
Subject: [PATCH] Fix wrong world AABB computation that caused broad-phase
 collision misses

---
 src/collision/shapes/CollisionShape.cpp | 50 +++++++++++++++++--------
 1 file changed, 34 insertions(+), 16 deletions(-)

diff --git a/src/collision/shapes/CollisionShape.cpp b/src/collision/shapes/CollisionShape.cpp
index d03b4a1c..9622e795 100644
--- a/src/collision/shapes/CollisionShape.cpp
+++ b/src/collision/shapes/CollisionShape.cpp
@@ -32,15 +32,19 @@
 using namespace reactphysics3d;
 
 // Constructor
-CollisionShape::CollisionShape(CollisionShapeName name, CollisionShapeType type) : mName(name), mType(type), mScaling(1.0, 1.0, 1.0) {
+CollisionShape::CollisionShape(CollisionShapeName name, CollisionShapeType type)
+               : mType(type), mName(name), mScaling(1.0, 1.0, 1.0) {
     
 }
 
-// Compute the world-space AABB of the collision shape given a transform
+// Compute the world-space AABB of the collision shape given a transform from shape
+// local-space to world-space. The technique is described in the book Real-Time Collision
+// Detection by Christer Ericson.
 /**
  * @param[out] aabb The axis-aligned bounding box (AABB) of the collision shape
  *                  computed in world-space coordinates
- * @param transform Transform used to compute the AABB of the collision shape
+ * @param transform Transform from shape local-space to world-space used to compute
+ *                  the AABB of the collision shape
  */
 void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const {
 
@@ -51,20 +55,34 @@ void CollisionShape::computeAABB(AABB& aabb, const Transform& transform) const {
     Vector3 maxBounds;
     getLocalBounds(minBounds, maxBounds);
 
-    // Rotate the local bounds according to the orientation of the body
-    Matrix3x3 worldAxis = transform.getOrientation().getMatrix().getAbsoluteMatrix();
-    Vector3 worldMinBounds(worldAxis.getColumn(0).dot(minBounds),
-                           worldAxis.getColumn(1).dot(minBounds),
-                           worldAxis.getColumn(2).dot(minBounds));
-    Vector3 worldMaxBounds(worldAxis.getColumn(0).dot(maxBounds),
-                           worldAxis.getColumn(1).dot(maxBounds),
-                           worldAxis.getColumn(2).dot(maxBounds));
+    const Vector3 translation = transform.getPosition();
+    Matrix3x3 matrix = transform.getOrientation().getMatrix();
+    Vector3 resultMin;
+    Vector3 resultMax;
 
-    // Compute the minimum and maximum coordinates of the rotated extents
-    Vector3 minCoordinates = transform.getPosition() + worldMinBounds;
-    Vector3 maxCoordinates = transform.getPosition() + worldMaxBounds;
+    // For each of the three axis
+    for (int i=0; i<3; i++) {
+
+        // Add translation component
+        resultMin[i] = translation[i];
+        resultMax[i] = translation[i];
+
+        for (int j=0; j<3; j++) {
+            decimal e = matrix[i][j] * minBounds[j];
+            decimal f = matrix[i][j] * maxBounds[j];
+
+            if (e < f) {
+                resultMin[i] += e;
+                resultMax[i] += f;
+            }
+            else {
+                resultMin[i] += f;
+                resultMax[i] += e;
+            }
+        }
+    }
 
     // Update the AABB with the new minimum and maximum coordinates
-    aabb.setMin(minCoordinates);
-    aabb.setMax(maxCoordinates);
+    aabb.setMin(resultMin);
+    aabb.setMax(resultMax);
 }