Fix wrong world AABB computation that caused broad-phase collision misses
This commit is contained in:
parent
fd427c0337
commit
6e322882eb
|
@ -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);
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue
Block a user