From 08ccec586aa42dcbab617d3168ab057de4d7b742 Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Fri, 18 Feb 2011 10:36:29 +0000 Subject: [PATCH] implementation of GJK and EPA collision detection algorithm continued git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@421 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- src/body/AABB.h | 23 +++++++++++++---------- src/body/BoundingSphere.cpp | 2 ++ src/body/BoundingSphere.h | 13 ++++++++++--- src/body/OBB.cpp | 2 +- src/collision/CollisionDetection.cpp | 5 +++-- src/collision/EPA/EPAAlgorithm.cpp | 2 ++ src/collision/GJK/GJKAlgorithm.cpp | 14 ++++++-------- src/collision/SAPAlgorithm.cpp | 2 +- src/collision/SATAlgorithm.h | 2 +- src/constants.h | 9 +++++---- src/reactphysics3d.h | 1 + 11 files changed, 45 insertions(+), 30 deletions(-) diff --git a/src/body/AABB.h b/src/body/AABB.h index 3884e4ec..a6b98455 100644 --- a/src/body/AABB.h +++ b/src/body/AABB.h @@ -122,34 +122,37 @@ inline void AABB::setExtent(unsigned int index, double extent) throw(std::invali } // Return the minimum position value on the given axis +// We consider the object margin here inline double AABB::getMinValueOnAxis(uint axis) const throw(std::invalid_argument) { switch (axis) { - case 0: return center.getX() - extent[0]; - case 1: return center.getY() - extent[1]; - case 2: return center.getZ() - extent[2]; + case 0: return center.getX() - extent[0] - OBJECT_MARGIN; + case 1: return center.getY() - extent[1] - OBJECT_MARGIN; + case 2: return center.getZ() - extent[2] - OBJECT_MARGIN; default: // The index value is not valid, we throw an exception throw std::invalid_argument("Exception : The index value has to be between 0 and 2"); } } // Return the maximum position value on the given axis +// We consider the object margin here inline double AABB::getMaxValueOnAxis(uint axis) const throw(std::invalid_argument) { switch (axis) { - case 0: return center.getX() + extent[0]; - case 1: return center.getY() + extent[1]; - case 2: return center.getZ() + extent[2]; + case 0: return center.getX() + extent[0] + OBJECT_MARGIN; + case 1: return center.getY() + extent[1] + OBJECT_MARGIN; + case 2: return center.getZ() + extent[2] + OBJECT_MARGIN; default: // The index value is not valid, we throw an exception throw std::invalid_argument("Exception : The index value has to be between 0 and 2"); } } -// Return true if the current AABB is overlapping is the AABB in argument +// Return true if the current AABB is overlapping with the AABB in argument // Two AABB overlap if they overlap in the three x, y and z axis at the same time inline bool AABB::testCollision(const AABB& aabb) const { Vector3D center2 = aabb.getCenter(); - if (std::abs(center.getX() - center2.getX()) > (extent[0] + aabb.getExtent(0))) return false; - if (std::abs(center.getY() - center2.getY()) > (extent[1] + aabb.getExtent(1))) return false; - if (std::abs(center.getZ() - center2.getZ()) > (extent[2] + aabb.getExtent(2))) return false; + double margin = 2 * OBJECT_MARGIN; + if (std::abs(center.getX() - center2.getX()) > (extent[0] + aabb.getExtent(0) + margin)) return false; + if (std::abs(center.getY() - center2.getY()) > (extent[1] + aabb.getExtent(1) + margin)) return false; + if (std::abs(center.getZ() - center2.getZ()) > (extent[2] + aabb.getExtent(2) + margin)) return false; return true; } diff --git a/src/body/BoundingSphere.cpp b/src/body/BoundingSphere.cpp index 4acdf8e4..b2004d93 100644 --- a/src/body/BoundingSphere.cpp +++ b/src/body/BoundingSphere.cpp @@ -53,6 +53,8 @@ void BoundingSphere::draw() const { // Draw in red glColor3f(1.0, 0.0, 0.0); + glTranslatef(center.getX(), center.getY(), center.getZ()); + // Draw the sphere glutWireSphere(radius, 50, 50); } diff --git a/src/body/BoundingSphere.h b/src/body/BoundingSphere.h index 0f36a4f3..05ff456b 100644 --- a/src/body/BoundingSphere.h +++ b/src/body/BoundingSphere.h @@ -88,11 +88,18 @@ inline void BoundingSphere::update(const Vector3D& newCenter, const Quaternion& // Return a support point in a given direction inline Vector3D BoundingSphere::getSupportPoint(const Vector3D& direction, double margin) const { - assert(direction.length() > 0.0); assert(margin >= 0.0); + double length = direction.length(); - // Return the support point of the sphere in the given direction - return center + (radius + margin) * direction.getUnit(); + // If the direction vector is not the zero vector + if (length > EPSILON) { + // Return the support point of the sphere in the given direction + return center + (radius + margin) * direction.getUnit(); + } + + // If the direction vector is the zero vector we return a point on the + // boundary of the sphere + return center + Vector3D(radius + margin, 0.0, 0.0); } }; // End of the ReactPhysics3D namespace diff --git a/src/body/OBB.cpp b/src/body/OBB.cpp index 023e09ec..426a948e 100644 --- a/src/body/OBB.cpp +++ b/src/body/OBB.cpp @@ -166,7 +166,7 @@ vector OBB::getExtremeVertices(const Vector3D& directionAxis) const { double projectionLength = directionAxis.dot(vertex-center) / directionAxis.length(); // If we found a bigger projection length - if (projectionLength > maxProjectionLength + EPSILON) { + if (projectionLength > maxProjectionLength + EPSILON_TEST) { maxProjectionLength = projectionLength; extremeVertices.clear(); extremeVertices.push_back(vertex); diff --git a/src/collision/CollisionDetection.cpp b/src/collision/CollisionDetection.cpp index baed6021..4430f791 100644 --- a/src/collision/CollisionDetection.cpp +++ b/src/collision/CollisionDetection.cpp @@ -25,6 +25,7 @@ // Libraries #include "CollisionDetection.h" #include "SAPAlgorithm.h" +#include "GJK/GJKAlgorithm.h" #include "SATAlgorithm.h" #include "../body/Body.h" #include "../body/OBB.h" @@ -44,7 +45,7 @@ CollisionDetection::CollisionDetection(PhysicsWorld* world) { broadPhaseAlgorithm = new SAPAlgorithm(); // Create the narrow-phase algorithm that will be used (Separating axis algorithm) - narrowPhaseAlgorithm = new SATAlgorithm(); + narrowPhaseAlgorithm = new GJKAlgorithm(); } // Destructor @@ -107,7 +108,7 @@ void CollisionDetection::computeAllContacts() { assert(contactInfo); // Compute one or several new contacts and add them into the physics world - computeContactSAT(contactInfo); + computeContactGJK(contactInfo); } } diff --git a/src/collision/EPA/EPAAlgorithm.cpp b/src/collision/EPA/EPAAlgorithm.cpp index 72ae50c4..f0cf7887 100644 --- a/src/collision/EPA/EPAAlgorithm.cpp +++ b/src/collision/EPA/EPAAlgorithm.cpp @@ -30,6 +30,8 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; +// TODO : Check that allocated memory is correctly deleted + // Constructor EPAAlgorithm::EPAAlgorithm() { diff --git a/src/collision/GJK/GJKAlgorithm.cpp b/src/collision/GJK/GJKAlgorithm.cpp index 9c723cec..10cd5386 100644 --- a/src/collision/GJK/GJKAlgorithm.cpp +++ b/src/collision/GJK/GJKAlgorithm.cpp @@ -37,6 +37,8 @@ // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; +// TODO : Check that allocated memory is correctly deleted + // Constructor GJKAlgorithm::GJKAlgorithm() { @@ -115,9 +117,8 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum pB = pB + (OBJECT_MARGIN / dist) * v; // Compute the contact info - Vector3D normal = (pB - pA).getUnit(); + Vector3D normal = v.getOpposite().getUnit(); double penetrationDepth = margin - dist; - assert(penetrationDepth > 0.0); contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(), normal, penetrationDepth, pA, pB); @@ -141,9 +142,8 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum pB = pB + (OBJECT_MARGIN / dist) * v; // Compute the contact info - Vector3D normal = (pB - pA).getUnit(); + Vector3D normal = v.getOpposite().getUnit(); double penetrationDepth = margin - dist; - assert(penetrationDepth > 0.0); contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(), normal, penetrationDepth, pA, pB); @@ -165,9 +165,8 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum pB = pB + (OBJECT_MARGIN / dist) * v; // Compute the contact info - Vector3D normal = (pB - pA).getUnit(); + Vector3D normal = v.getOpposite().getUnit(); double penetrationDepth = margin - dist; - assert(penetrationDepth > 0.0); contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(), normal, penetrationDepth, pA, pB); @@ -197,9 +196,8 @@ bool GJKAlgorithm::testCollision(const NarrowBoundingVolume* const boundingVolum pB = pB + (OBJECT_MARGIN / dist) * v; // Compute the contact info - Vector3D normal = (pB - pA).getUnit(); + Vector3D normal = v.getOpposite().getUnit(); double penetrationDepth = margin - dist; - assert(penetrationDepth > 0.0); contactInfo = new ContactInfo(boundingVolume1->getBodyPointer(), boundingVolume2->getBodyPointer(), normal, penetrationDepth, pA, pB); diff --git a/src/collision/SAPAlgorithm.cpp b/src/collision/SAPAlgorithm.cpp index 5472c1b4..801510a1 100644 --- a/src/collision/SAPAlgorithm.cpp +++ b/src/collision/SAPAlgorithm.cpp @@ -142,7 +142,7 @@ void SAPAlgorithm::computePossibleCollisionPairs(vector addedBodies, vect variance[i] = esperanceSquare[i] - esperance[i] * esperance[i] / nbAABBs; } - // Update the sorted Axis according to the axis with the largest variance + // Update the sorted axis according to the axis with the largest variance sortAxis = 0; if (variance[1] > variance[0]) sortAxis = 1; if (variance[2] > variance[sortAxis]) sortAxis = 2; diff --git a/src/collision/SATAlgorithm.h b/src/collision/SATAlgorithm.h index e693e980..875d1313 100644 --- a/src/collision/SATAlgorithm.h +++ b/src/collision/SATAlgorithm.h @@ -63,7 +63,7 @@ class SATAlgorithm : public NarrowPhaseAlgorithm { }; // Return the contact normal with the correct sign (from obb1 toward obb2). "axis" is the axis vector direction where the -// collision occur and "distanceOfOBBs" is the vector (obb2.center - obb1.center). +// collision occurs and "distanceOfOBBs" is the vector (obb2.center - obb1.center). inline Vector3D SATAlgorithm::computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const { if (distanceOfOBBs.dot(axis) >= 0.0) { return axis; diff --git a/src/constants.h b/src/constants.h index a4ddee22..eeac851e 100644 --- a/src/constants.h +++ b/src/constants.h @@ -33,17 +33,18 @@ typedef unsigned int uint; // Mathematical constants -const double EPSILON = 0.00001; // Epsilon value to avoid numerical errors -const double ONE_MINUS_EPSILON = 0.99999; // 1 - espilon +const double EPSILON = 1.0e-10; // Epsilon value +const double MACHINE_EPSILON = DBL_EPSILON; // Machine epsilon +const double EPSILON_TEST = 0.00001; // TODO : Try not to use this value +const double ONE_MINUS_EPSILON_TEST = 0.99999; // TODO : Try not to use this value const double INFINITY_CONST = std::numeric_limits::infinity(); // Infinity constant const double PI = 3.14159265; // Pi constant -const double MACHINE_EPSILON = DBL_EPSILON; // Physics Engine constants const double DEFAULT_TIMESTEP = 0.002; // GJK Algorithm parameters -const double OBJECT_MARGIN = 0.1; // Object margin for the hybrid GJK algorithm +const double OBJECT_MARGIN = 0.04; // Object margin for collision detection // Contact constants const double FRICTION_COEFFICIENT = 0.4; // Friction coefficient diff --git a/src/reactphysics3d.h b/src/reactphysics3d.h index 153e01d2..c9bf8cb3 100644 --- a/src/reactphysics3d.h +++ b/src/reactphysics3d.h @@ -40,6 +40,7 @@ #include "engine/PhysicsEngine.h" #include "body/BoundingVolume.h" #include "body/OBB.h" +#include "body/BoundingSphere.h" #include "body/AABB.h" // Alias to the ReactPhysics3D namespace