From 9f4e63361ff4a62a02838d9f7ca23d280d7a8081 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Mon, 19 Jan 2015 21:55:18 +0100 Subject: [PATCH] Small optimization in EPA algorithm --- .../narrowphase/EPA/EPAAlgorithm.cpp | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp index 9ad0238c..1f0bcc9a 100644 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp +++ b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp @@ -103,8 +103,8 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple // Matrix that transform a direction from local // space of body 1 into local space of body 2 - Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * - transform1.getOrientation().getMatrix(); + Quaternion rotateToBody2 = transform2.getOrientation().getInverse() * + transform1.getOrientation(); // Get the simplex computed previously by the GJK algorithm unsigned int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points); @@ -150,13 +150,10 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple // v2 and v3 Quaternion rotationQuat(d.x * sin60, d.y * sin60, d.z * sin60, 0.5); - // Construct the corresponding rotation matrix - Matrix3x3 rotationMat = rotationQuat.getMatrix(); - // Compute the vector v1, v2, v3 Vector3 v1 = d.cross(Vector3(minAxis == 0, minAxis == 1, minAxis == 2)); - Vector3 v2 = rotationMat * v1; - Vector3 v3 = rotationMat * v2; + Vector3 v2 = rotationQuat * v1; + Vector3 v3 = rotationQuat * v2; // Compute the support point in the direction of v1 suppPointsA[2] = collisionShape1->getLocalSupportPointWithMargin(v1); @@ -281,10 +278,10 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * n); points[4] = suppPointsA[4] - suppPointsB[4]; - TriangleEPA* face0; - TriangleEPA* face1; - TriangleEPA* face2; - TriangleEPA* face3; + TriangleEPA* face0 = NULL; + TriangleEPA* face1 = NULL; + TriangleEPA* face2 = NULL; + TriangleEPA* face3 = NULL; // If the origin is in the first tetrahedron if (isOriginInTetrahedron(points[0], points[1],