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