mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Small optimization in EPA algorithm

Browse Source
This commit is contained in:
Daniel Chappuis 2015-01-19 21:55:18 +01:00
parent 54d8b8518e
commit 9f4e63361f
1 changed files with 8 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
src/collision/narrowphase/EPA/EPAAlgorithm.cpp
View File

@ -103,8 +103,8 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// Matrix that transform a direction from local // Matrix that transform a direction from local
// space of body 1 into local space of body 2 // space of body 1 into local space of body 2
Matrix3x3 rotateToBody2 = transform2.getOrientation().getMatrix().getTranspose() * Quaternion rotateToBody2 = transform2.getOrientation().getInverse() *
transform1.getOrientation().getMatrix(); transform1.getOrientation();
// Get the simplex computed previously by the GJK algorithm // Get the simplex computed previously by the GJK algorithm
unsigned int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points); unsigned int nbVertices = simplex.getSimplex(suppPointsA, suppPointsB, points);
@ -150,13 +150,10 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
// v2 and v3 // v2 and v3
Quaternion rotationQuat(d.x * sin60, d.y * sin60, d.z * sin60, 0.5); 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 // Compute the vector v1, v2, v3
Vector3 v1 = d.cross(Vector3(minAxis == 0, minAxis == 1, minAxis == 2)); Vector3 v1 = d.cross(Vector3(minAxis == 0, minAxis == 1, minAxis == 2));
Vector3 v2 = rotationMat * v1; Vector3 v2 = rotationQuat * v1;
Vector3 v3 = rotationMat * v2; Vector3 v3 = rotationQuat * v2;
// Compute the support point in the direction of v1 // Compute the support point in the direction of v1
suppPointsA[2] = collisionShape1->getLocalSupportPointWithMargin(v1); suppPointsA[2] = collisionShape1->getLocalSupportPointWithMargin(v1);
@ -281,10 +278,10 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple
collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * n); collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * n);
points[4] = suppPointsA[4] - suppPointsB[4]; points[4] = suppPointsA[4] - suppPointsB[4];
TriangleEPA* face0; TriangleEPA* face0 = NULL;
TriangleEPA* face1; TriangleEPA* face1 = NULL;
TriangleEPA* face2; TriangleEPA* face2 = NULL;
TriangleEPA* face3; TriangleEPA* face3 = NULL;
// If the origin is in the first tetrahedron // If the origin is in the first tetrahedron
if (isOriginInTetrahedron(points[0], points[1], if (isOriginInTetrahedron(points[0], points[1],