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

@@ -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 0: return center.getX() - extent[0] - OBJECT_MARGIN;
-        case 1: return center.getY() - extent[1];
+        case 1: return center.getY() - extent[1] - OBJECT_MARGIN;
-        case 2: return center.getZ() - extent[2];
+        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 0: return center.getX() + extent[0] + OBJECT_MARGIN;
-        case 1: return center.getY() + extent[1];
+        case 1: return center.getY() + extent[1] + OBJECT_MARGIN;
-        case 2: return center.getZ() + extent[2];
+        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;
+    double margin = 2 * OBJECT_MARGIN;
-    if (std::abs(center.getY() - center2.getY()) > (extent[1] + aabb.getExtent(1))) return false;
+    if (std::abs(center.getX() - center2.getX()) > (extent[0] + aabb.getExtent(0) + margin)) return false;
-    if (std::abs(center.getZ() - center2.getZ()) > (extent[2] + aabb.getExtent(2))) 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;
 }
 

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);
 }

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
+    // If the direction vector is not the zero vector
-    return center + (radius + margin) * direction.getUnit();
+    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

src/body/OBB.cpp

@@ -166,7 +166,7 @@ vector<Vector3D> 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);

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);
     }
 }
 

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() {
 

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);
 

src/collision/SAPAlgorithm.cpp

@@ -142,7 +142,7 @@ void SAPAlgorithm::computePossibleCollisionPairs(vector<Body*> 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;

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;

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 EPSILON = 1.0e-10;                                             // Epsilon value
-const double ONE_MINUS_EPSILON = 0.99999;                                   // 1 - espilon
+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<double>::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

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