From b43f875cef1af973fa18d0d6883e99939e24fe41 Mon Sep 17 00:00:00 2001 From: Daniel Chappuis Date: Sun, 3 Mar 2013 16:24:46 +0100 Subject: [PATCH] Improve GJK robustness for spheres by integrating the radius into the object margin --- .../narrowphase/EPA/EPAAlgorithm.cpp | 44 +++++++++---------- .../narrowphase/GJK/GJKAlgorithm.cpp | 28 ++++++------ src/collision/shapes/BoxShape.h | 31 ++++++++++--- src/collision/shapes/CollisionShape.h | 10 ++++- src/collision/shapes/ConeShape.cpp | 30 ++++++++----- src/collision/shapes/ConeShape.h | 15 ++++++- src/collision/shapes/CylinderShape.cpp | 30 ++++++++----- src/collision/shapes/CylinderShape.h | 15 ++++++- src/collision/shapes/SphereShape.h | 37 ++++++++++++---- 9 files changed, 158 insertions(+), 82 deletions(-) diff --git a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp index 046d81be..bca36156 100644 --- a/src/collision/narrowphase/EPA/EPAAlgorithm.cpp +++ b/src/collision/narrowphase/EPA/EPAAlgorithm.cpp @@ -158,24 +158,21 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple Vector3 v3 = rotationMat * v2; // Compute the support point in the direction of v1 - suppPointsA[2] = collisionShape1->getLocalSupportPoint(v1, OBJECT_MARGIN); - suppPointsB[2] = body2Tobody1 * collisionShape2->getLocalSupportPoint(rotateToBody2 * - (-v1), - OBJECT_MARGIN); + suppPointsA[2] = collisionShape1->getLocalSupportPointWithMargin(v1); + suppPointsB[2] = body2Tobody1 * + collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v1)); points[2] = suppPointsA[2] - suppPointsB[2]; // Compute the support point in the direction of v2 - suppPointsA[3] = collisionShape1->getLocalSupportPoint(v2, OBJECT_MARGIN); - suppPointsB[3] = body2Tobody1 * collisionShape2->getLocalSupportPoint(rotateToBody2 * - (-v2), - OBJECT_MARGIN); + suppPointsA[3] = collisionShape1->getLocalSupportPointWithMargin(v2); + suppPointsB[3] = body2Tobody1 * + collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v2)); points[3] = suppPointsA[3] - suppPointsB[3]; // Compute the support point in the direction of v3 - suppPointsA[4] = collisionShape1->getLocalSupportPoint(v3, OBJECT_MARGIN); - suppPointsB[4] = body2Tobody1 * collisionShape2->getLocalSupportPoint(rotateToBody2 * - (-v3), - OBJECT_MARGIN); + suppPointsA[4] = collisionShape1->getLocalSupportPointWithMargin(v3); + suppPointsB[4] = body2Tobody1 * + collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * (-v3)); points[4] = suppPointsA[4] - suppPointsB[4]; // Now we have an hexahedron (two tetrahedron glued together). We can simply keep the @@ -272,14 +269,13 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple Vector3 n = v1.cross(v2); // Compute the two new vertices to obtain a hexahedron - suppPointsA[3] = collisionShape1->getLocalSupportPoint(n, OBJECT_MARGIN); - suppPointsB[3] = body2Tobody1 * collisionShape2->getLocalSupportPoint(rotateToBody2 * - (-n), - OBJECT_MARGIN); + suppPointsA[3] = collisionShape1->getLocalSupportPointWithMargin(n); + suppPointsB[3] = body2Tobody1 * + collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * (-n)); points[3] = suppPointsA[3] - suppPointsB[3]; - suppPointsA[4] = collisionShape1->getLocalSupportPoint(-n, OBJECT_MARGIN); - suppPointsB[4] = body2Tobody1 * collisionShape2->getLocalSupportPoint(rotateToBody2 * n, - OBJECT_MARGIN); + suppPointsA[4] = collisionShape1->getLocalSupportPointWithMargin(-n); + suppPointsB[4] = body2Tobody1 * + collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * n); points[4] = suppPointsA[4] - suppPointsB[4]; // Construct the triangle faces @@ -350,11 +346,11 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(const Simplex& simple // Compute the support point of the Minkowski // difference (A-B) in the closest point direction - suppPointsA[nbVertices] = collisionShape1->getLocalSupportPoint( - triangle->getClosestPoint(), OBJECT_MARGIN); - suppPointsB[nbVertices] = body2Tobody1 * collisionShape2->getLocalSupportPoint( - rotateToBody2 * (-triangle->getClosestPoint()), - OBJECT_MARGIN); + suppPointsA[nbVertices] = collisionShape1->getLocalSupportPointWithMargin( + triangle->getClosestPoint()); + suppPointsB[nbVertices] = body2Tobody1 * + collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * + (-triangle->getClosestPoint())); points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices]; int indexNewVertex = nbVertices; diff --git a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp index ed1dbd9b..2ecdd919 100644 --- a/src/collision/narrowphase/GJK/GJKAlgorithm.cpp +++ b/src/collision/narrowphase/GJK/GJKAlgorithm.cpp @@ -81,7 +81,7 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1, transform1.getOrientation().getMatrix(); // Initialize the margin (sum of margins of both objects) - decimal margin = 2 * OBJECT_MARGIN; + decimal margin = collisionShape1->getMargin() + collisionShape2->getMargin(); decimal marginSquare = margin * margin; assert(margin > 0.0); @@ -97,8 +97,9 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1, do { // Compute the support points for original objects (without margins) A and B - suppA = collisionShape1->getLocalSupportPoint(-v); - suppB = body2Tobody1 * collisionShape2->getLocalSupportPoint(rotateToBody2 * v); + suppA = collisionShape1->getLocalSupportPointWithoutMargin(-v); + suppB = body2Tobody1 * + collisionShape2->getLocalSupportPointWithoutMargin(rotateToBody2 * v); // Compute the support point for the Minkowski difference A-B w = suppA - suppB; @@ -125,8 +126,8 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1, // object with the margins decimal dist = sqrt(distSquare); assert(dist > 0.0); - pA = (pA - (OBJECT_MARGIN / dist) * v); - pB = body2Tobody1.inverse() * (pB + (OBJECT_MARGIN / dist) * v); + pA = (pA - (collisionShape1->getMargin() / dist) * v); + pB = body2Tobody1.inverse() * (pB + (collisionShape2->getMargin() / dist) * v); // Compute the contact info Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit()); @@ -157,8 +158,8 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1, // object with the margins decimal dist = sqrt(distSquare); assert(dist > 0.0); - pA = (pA - (OBJECT_MARGIN / dist) * v); - pB = body2Tobody1.inverse() * (pB + (OBJECT_MARGIN / dist) * v); + pA = (pA - (collisionShape1->getMargin() / dist) * v); + pB = body2Tobody1.inverse() * (pB + (collisionShape2->getMargin() / dist) * v); // Compute the contact info Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit()); @@ -187,8 +188,8 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1, // object with the margins decimal dist = sqrt(distSquare); assert(dist > 0.0); - pA = (pA - (OBJECT_MARGIN / dist) * v); - pB = body2Tobody1.inverse() * (pB + (OBJECT_MARGIN / dist) * v); + pA = (pA - (collisionShape1->getMargin() / dist) * v); + pB = body2Tobody1.inverse() * (pB + (collisionShape2->getMargin() / dist) * v); // Compute the contact info Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit()); @@ -224,8 +225,8 @@ bool GJKAlgorithm::testCollision(const CollisionShape* collisionShape1, // object with the margins decimal dist = sqrt(distSquare); assert(dist > 0.0); - pA = (pA - (OBJECT_MARGIN / dist) * v); - pB = body2Tobody1.inverse() * (pB + (OBJECT_MARGIN / dist) * v); + pA = (pA - (collisionShape1->getMargin() / dist) * v); + pB = body2Tobody1.inverse() * (pB + (collisionShape2->getMargin() / dist) * v); // Compute the contact info Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit()); @@ -282,9 +283,8 @@ bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const CollisionShap do { // Compute the support points for the enlarged object A and B - suppA = collisionShape1->getLocalSupportPoint(-v, OBJECT_MARGIN); - suppB = body2ToBody1 * collisionShape2->getLocalSupportPoint(rotateToBody2 * v, - OBJECT_MARGIN); + suppA = collisionShape1->getLocalSupportPointWithMargin(-v); + suppB = body2ToBody1 * collisionShape2->getLocalSupportPointWithMargin(rotateToBody2 * v); // Compute the support point for the Minkowski difference A-B w = suppA - suppB; diff --git a/src/collision/shapes/BoxShape.h b/src/collision/shapes/BoxShape.h index 7e08b818..2710da7a 100644 --- a/src/collision/shapes/BoxShape.h +++ b/src/collision/shapes/BoxShape.h @@ -79,8 +79,14 @@ class BoxShape : public CollisionShape { // Return the local extents in x,y and z direction virtual Vector3 getLocalExtents(decimal margin=0.0) const; - // Return a local support point in a given direction - virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const; + // Return the margin distance around the shape + virtual decimal getMargin() const; + + // Return a local support point in a given direction with the object margin + virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const; + + // Return a local support point in a given direction without the object margin + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const; // Return the local inertia tensor of the collision shape virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; @@ -104,11 +110,18 @@ inline void BoxShape::setExtent(const Vector3& extent) { // Return the local extents of the box (half-width) in x,y and z local direction // This method is used to compute the AABB of the box inline Vector3 BoxShape::getLocalExtents(decimal margin) const { - return mExtent + Vector3(margin, margin, margin); + return mExtent + Vector3(getMargin(), getMargin(), getMargin()); } -// Return a local support point in a given direction -inline Vector3 BoxShape::getLocalSupportPoint(const Vector3& direction, decimal margin) const { +// Return the margin distance around the shape +inline decimal BoxShape::getMargin() const { + return OBJECT_MARGIN; +} + +// Return a local support point in a given direction with the object margin +inline Vector3 BoxShape::getLocalSupportPointWithMargin(const Vector3& direction) const { + + decimal margin = getMargin(); assert(margin >= 0.0); return Vector3(direction.x < 0.0 ? -mExtent.x - margin : mExtent.x + margin, @@ -116,6 +129,14 @@ inline Vector3 BoxShape::getLocalSupportPoint(const Vector3& direction, decimal direction.z < 0.0 ? -mExtent.z - margin : mExtent.z + margin); } +// Return a local support point in a given direction without the objec margin +inline Vector3 BoxShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { + + return Vector3(direction.x < 0.0 ? -mExtent.x : mExtent.x, + direction.y < 0.0 ? -mExtent.y : mExtent.y, + direction.z < 0.0 ? -mExtent.z : mExtent.z); +} + } #endif diff --git a/src/collision/shapes/CollisionShape.h b/src/collision/shapes/CollisionShape.h index 63f1ef7a..cf392343 100644 --- a/src/collision/shapes/CollisionShape.h +++ b/src/collision/shapes/CollisionShape.h @@ -76,12 +76,18 @@ class CollisionShape { // Return the type of the collision shapes CollisionShapeType getType() const; - // Return a local support point in a given direction - virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const=0; + // Return a local support point in a given direction with the object margin + virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const=0; + + // Return a local support point in a given direction without the object margin + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const=0; // Return the local extents in x,y and z direction virtual Vector3 getLocalExtents(decimal margin=0.0) const=0; + // Return the margin distance around the shape + virtual decimal getMargin() const=0; + // Return the local inertia tensor of the collision shapes virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const=0; }; diff --git a/src/collision/shapes/ConeShape.cpp b/src/collision/shapes/ConeShape.cpp index f13ebebf..55f56e35 100644 --- a/src/collision/shapes/ConeShape.cpp +++ b/src/collision/shapes/ConeShape.cpp @@ -58,9 +58,24 @@ ConeShape::~ConeShape() { } -// Return a local support point in a given direction -inline Vector3 ConeShape::getLocalSupportPoint(const Vector3& direction, decimal margin) const { - assert(margin >= 0.0); +// Return a local support point in a given direction with the object margin +inline Vector3 ConeShape::getLocalSupportPointWithMargin(const Vector3& direction) const { + + // Compute the support point without the margin + Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction); + + // Add the margin to the support point + Vector3 unitVec(0.0, -1.0, 0.0); + if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) { + unitVec = direction.getUnit(); + } + supportPoint += unitVec * getMargin(); + + return supportPoint; +} + +// Return a local support point in a given direction without the object margin +inline Vector3 ConeShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { const Vector3& v = direction; decimal sinThetaTimesLengthV = mSinTheta * v.length(); @@ -80,15 +95,6 @@ inline Vector3 ConeShape::getLocalSupportPoint(const Vector3& direction, decimal } } - // Add the margin to the support point - if (margin != 0.0) { - Vector3 unitVec(0.0, -1.0, 0.0); - if (v.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) { - unitVec = v.getUnit(); - } - supportPoint += unitVec * margin; - } - return supportPoint; } diff --git a/src/collision/shapes/ConeShape.h b/src/collision/shapes/ConeShape.h index 413fa283..8785e61a 100644 --- a/src/collision/shapes/ConeShape.h +++ b/src/collision/shapes/ConeShape.h @@ -88,8 +88,11 @@ class ConeShape : public CollisionShape { // Set the height void setHeight(decimal height); - // Return a support point in a given direction - virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const; + // Return a local support point in a given direction with the object margin + virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const; + + // Return a local support point in a given direction without the object margin + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const; // Return the local extents in x,y and z direction virtual Vector3 getLocalExtents(decimal margin=0.0) const; @@ -97,6 +100,9 @@ class ConeShape : public CollisionShape { // Return the local inertia tensor of the collision shape virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; + // Return the margin distance around the shape + virtual decimal getMargin() const; + #ifdef VISUAL_DEBUG // Draw the sphere (only for testing purpose) virtual void draw() const; @@ -143,6 +149,11 @@ inline void ConeShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass 0.0, 0.0, 0.0, diagXZ); } +// Return the margin distance around the shape +inline decimal ConeShape::getMargin() const { + return OBJECT_MARGIN; +} + } #endif diff --git a/src/collision/shapes/CylinderShape.cpp b/src/collision/shapes/CylinderShape.cpp index 433ba8aa..abb8a175 100644 --- a/src/collision/shapes/CylinderShape.cpp +++ b/src/collision/shapes/CylinderShape.cpp @@ -53,9 +53,24 @@ CylinderShape::~CylinderShape() { } -// Return a local support point in a given direction -Vector3 CylinderShape::getLocalSupportPoint(const Vector3& direction, decimal margin) const { - assert(margin >= 0.0); +// Return a local support point in a given direction with the object margin +Vector3 CylinderShape::getLocalSupportPointWithMargin(const Vector3& direction) const { + + // Compute the support point without the margin + Vector3 supportPoint = getLocalSupportPointWithoutMargin(direction); + + // Add the margin to the support point + Vector3 unitVec(0.0, 1.0, 0.0); + if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) { + unitVec = direction.getUnit(); + } + supportPoint += unitVec * getMargin(); + + return supportPoint; +} + +// Return a local support point in a given direction without the object margin +Vector3 CylinderShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { Vector3 supportPoint(0.0, 0.0, 0.0); decimal uDotv = direction.y; @@ -72,15 +87,6 @@ Vector3 CylinderShape::getLocalSupportPoint(const Vector3& direction, decimal ma else supportPoint.y = mHalfHeight; } - // Add the margin to the support point - if (margin != 0.0) { - Vector3 unitVec(0.0, 1.0, 0.0); - if (direction.lengthSquare() > MACHINE_EPSILON * MACHINE_EPSILON) { - unitVec = direction.getUnit(); - } - supportPoint += unitVec * margin; - } - return supportPoint; } diff --git a/src/collision/shapes/CylinderShape.h b/src/collision/shapes/CylinderShape.h index fec53379..0fec1c14 100644 --- a/src/collision/shapes/CylinderShape.h +++ b/src/collision/shapes/CylinderShape.h @@ -84,8 +84,11 @@ class CylinderShape : public CollisionShape { // Set the height void setHeight(decimal height); - // Return a support point in a given direction - virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const; + // Return a local support point in a given direction with the object margin + virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const; + + // Return a local support point in a given direction without the object margin + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const; // Return the local extents in x,y and z direction virtual Vector3 getLocalExtents(decimal margin=0.0) const; @@ -93,6 +96,9 @@ class CylinderShape : public CollisionShape { // Return the local inertia tensor of the collision shape virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; + // Return the margin distance around the shape + virtual decimal getMargin() const; + #ifdef VISUAL_DEBUG // Draw the sphere (only for testing purpose) virtual void draw() const; @@ -133,6 +139,11 @@ inline void CylinderShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal 0.0, 0.0, diag); } +// Return the margin distance around the shape +inline decimal CylinderShape::getMargin() const { + return OBJECT_MARGIN; +} + } #endif diff --git a/src/collision/shapes/SphereShape.h b/src/collision/shapes/SphereShape.h index cb4ea0e4..a387aed6 100644 --- a/src/collision/shapes/SphereShape.h +++ b/src/collision/shapes/SphereShape.h @@ -72,8 +72,11 @@ class SphereShape : public CollisionShape { // Set the radius of the sphere void setRadius(decimal radius); - // Return a local support point in a given direction - virtual Vector3 getLocalSupportPoint(const Vector3& direction, decimal margin=0.0) const; + // Return a local support point in a given direction with the object margin + virtual Vector3 getLocalSupportPointWithMargin(const Vector3& direction) const; + + // Return a local support point in a given direction without the object margin + virtual Vector3 getLocalSupportPointWithoutMargin(const Vector3& direction) const; // Return the local extents in x,y and z direction virtual Vector3 getLocalExtents(decimal margin=0.0) const; @@ -81,6 +84,9 @@ class SphereShape : public CollisionShape { // Return the local inertia tensor of the collision shape virtual void computeLocalInertiaTensor(Matrix3x3& tensor, decimal mass) const; + // Return the margin distance around the shape + virtual decimal getMargin() const; + #ifdef VISUAL_DEBUG // Draw the sphere (only for testing purpose) virtual void draw() const; @@ -97,20 +103,28 @@ inline void SphereShape::setRadius(decimal radius) { mRadius = radius; } -// Return a local support point in a given direction -inline Vector3 SphereShape::getLocalSupportPoint(const Vector3& direction, decimal margin) const { - assert(margin >= 0.0); - decimal length = direction.length(); +// Return a local support point in a given direction with the object margin +inline Vector3 SphereShape::getLocalSupportPointWithMargin(const Vector3& direction) const { + + decimal margin = getMargin(); // If the direction vector is not the zero vector - if (length > 0.0) { + if (direction.lengthSquare() >= MACHINE_EPSILON * MACHINE_EPSILON) { + // Return the support point of the sphere in the given direction - return (mRadius + margin) * direction.getUnit(); + return margin * direction.getUnit(); } // If the direction vector is the zero vector we return a point on the // boundary of the sphere - return Vector3(0, mRadius + margin, 0); + return Vector3(0, margin, 0); +} + +// Return a local support point in a given direction without the object margin +inline Vector3 SphereShape::getLocalSupportPointWithoutMargin(const Vector3& direction) const { + + // Return the center of the sphere (the radius is taken into account in the object margin) + return Vector3(0.0, 0.0, 0.0); } // Return the local extents of the collision shape (half-width) in x,y and z local direction @@ -127,6 +141,11 @@ inline void SphereShape::computeLocalInertiaTensor(Matrix3x3& tensor, decimal ma 0.0, 0.0, diag); } +// Return the margin distance around the shape +inline decimal SphereShape::getMargin() const { + return mRadius + OBJECT_MARGIN; +} + } #endif