implementation of GJK and EPA collision detection algorithm continued

git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@423 92aac97c-a6ce-11dd-a772-7fcde58d38e6

src/collision/BroadPhaseAlgorithm.h

@@ -22,8 +22,8 @@
 * THE SOFTWARE.                                                                 *
 ********************************************************************************/
 
-#ifndef BROADPHASEALGORITHM_H
+#ifndef BROAD_PHASE_ALGORITHM_H
-#define BROADPHASEALGORITHM_H
+#define BROAD_PHASE_ALGORITHM_H
 
 // Libraries
 #include "../body/BoundingVolume.h"

src/collision/CollisionDetection.h

@@ -22,8 +22,8 @@
 * THE SOFTWARE.                                                                 *
 ********************************************************************************/
 
-#ifndef COLLISIONDETECTION_H
+#ifndef COLLISION_DETECTION_H
-#define COLLISIONDETECTION_H
+#define COLLISION_DETECTION_H
 
 // Libraries
 #include "BroadPhaseAlgorithm.h"

src/collision/ContactInfo.h

@@ -22,8 +22,8 @@
 * THE SOFTWARE.                                                                 *
 ********************************************************************************/
 
-#ifndef CONTACTINFO_H
+#ifndef CONTACT_INFO_H
-#define	CONTACTINFO_H
+#define	CONTACT_INFO_H
 
 // Libraries
 #include "../body/OBB.h"

src/collision/EPA/EPAAlgorithm.cpp

@@ -323,7 +323,7 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
 
             // Compute the support point of the Minkowski difference (A-B) in the closest point direction
             suppPointsA[nbVertices] = boundingVolume1->getSupportPoint(triangle->getClosestPoint(), OBJECT_MARGIN);
-            suppPointsB[nbVertices] = boundingVolume2->getSupportPoint(triangle->getClosestPoint().getOpposite());
+            suppPointsB[nbVertices] = boundingVolume2->getSupportPoint(triangle->getClosestPoint().getOpposite(), OBJECT_MARGIN);
             points[nbVertices] = suppPointsA[nbVertices] - suppPointsB[nbVertices];
 
             int indexNewVertex = nbVertices;
@@ -360,7 +360,6 @@ bool EPAAlgorithm::computePenetrationDepthAndContactPoints(Simplex simplex, cons
             // polytope is always convex
             while(i != triangleStore.getNbTriangles()) {
                 TriangleEPA* newTriangle = &triangleStore[i];
-
                 addFaceCandidate(newTriangle, triangleHeap, nbTriangles, upperBoundSquarePenDepth);
                 i++;
             }

src/collision/EPA/EPAAlgorithm.h

@@ -22,8 +22,8 @@
 * THE SOFTWARE.                                                                 *
 ********************************************************************************/
 
-#ifndef EPAAlgorithm_H
+#ifndef EPA_ALGORITHM_H
-#define EPAAlgorithm_H
+#define EPA_ALGORITHM_H
 
 // Libraries
 #include "../GJK/Simplex.h"

src/collision/EPA/EdgeEPA.cpp

@@ -82,7 +82,7 @@ void EdgeEPA::halfLink(EdgeEPA edge) {
 }
 
 // Compute the silhouette
-bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesStore triangleStore) {
+bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesStore& triangleStore) {
     // If the edge has not already been visited
     if (!ownerTriangle->getIsObsolete()) {
         // If the triangle of this edge is not visible from the given point
@@ -104,7 +104,7 @@ bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesS
 
         int backup = triangleStore.getNbTriangles();
 
-        if(!ownerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(index)).computeSilhouette(vertices, index, triangleStore)) {
+        if(!ownerTriangle->getAdjacentEdge(indexOfNextCounterClockwiseEdge(this->index)).computeSilhouette(vertices, index, triangleStore)) {
             ownerTriangle->setIsObsolete(false);
 
             TriangleEPA* triangle = triangleStore.newTriangle(vertices, index, getTarget(), getSource());
@@ -117,7 +117,7 @@ bool EdgeEPA::computeSilhouette(const Vector3D* vertices, uint index, TrianglesS
 
             return false;
         }
-        else if (!ownerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(index)).computeSilhouette(vertices, index, triangleStore)) {
+        else if (!ownerTriangle->getAdjacentEdge(indexOfPreviousCounterClockwiseEdge(this->index)).computeSilhouette(vertices, index, triangleStore)) {
             ownerTriangle->setIsObsolete(false);
 
             triangleStore.setNbTriangles(backup);

src/collision/EPA/EdgeEPA.h

@@ -22,8 +22,8 @@
 * THE SOFTWARE.                                                                 *
 ********************************************************************************/
 
-#ifndef EDGEEPA_H
+#ifndef EDGE_EPA_H
-#define EDGEEPA_H
+#define EDGE_EPA_H
 
 
 // Libraries
@@ -49,9 +49,9 @@ class EdgeEPA {
                                             // The edge with index i connect triangle vertices i and (i+1 % 3)
 
     public:
-        EdgeEPA();                                          // Constructor
+        EdgeEPA();                                                      // Constructor
-        EdgeEPA(TriangleEPA* ownerTriangle, int index);     // Constructor
+        EdgeEPA(TriangleEPA* ownerTriangle, int index);                 // Constructor
-        ~EdgeEPA();                                         // Destructor
+        ~EdgeEPA();                                                     // Destructor
         TriangleEPA* getOwnerTriangle() const;                          // Return the pointer to the owner triangle
         int getIndex() const;                                           // Return the index of the edge in the triangle
         uint getSource() const;                                         // Return index of the source vertex of the edge
@@ -59,7 +59,7 @@ class EdgeEPA {
         bool link(EdgeEPA edge);                                        // Link the edge with another one
         void halfLink(EdgeEPA edge);                                    // Half link the edge with another one
         bool computeSilhouette(const Vector3D* vertices, uint index,
-                               TrianglesStore triangleStore);           // Compute the recursive silhouette algorithm
+                               TrianglesStore& triangleStore);          // Compute the recursive silhouette algorithm
 };
 
 

src/collision/EPA/TriangleEPA.h

@@ -22,8 +22,8 @@
 * THE SOFTWARE.                                                                 *
 ********************************************************************************/
 
-#ifndef TRIANGLEEPA_H
+#ifndef TRIANGLE_EPA_H
-#define TRIANGLEEPA_H
+#define TRIANGLE_EPA_H
 
 // Libraries
 #include "../../mathematics/mathematics.h"

src/collision/EPA/TrianglesStore.h

@@ -22,8 +22,8 @@
 * THE SOFTWARE.                                                                 *
 ********************************************************************************/
 
-#ifndef TRIANGLESSTORE_H
+#ifndef TRIANGLES_STORE_H
-#define TRIANGLESSTORE_H
+#define TRIANGLES_STORE_H
 
 #include "TriangleEPA.h"
 

src/collision/GJK/GJKAlgorithm.h

@@ -22,8 +22,8 @@
 * THE SOFTWARE.                                                                 *
 ********************************************************************************/
 
-#ifndef GJKALGORITHM_H
+#ifndef GJK_ALGORITHM_H
-#define GJKALGORITHM_H
+#define GJK_ALGORITHM_H
 
 // Libraries
 #include "../NarrowPhaseAlgorithm.h"

src/collision/NarrowPhaseAlgorithm.h

@@ -22,8 +22,8 @@
 * THE SOFTWARE.                                                                 *
 ********************************************************************************/
 
-#ifndef NARROWPHASEALGORITHM_H
+#ifndef NARROW_PHASE_ALGORITHM_H
-#define NARROWPHASEALGORITHM_H
+#define NARROW_PHASE_ALGORITHM_H
 
 // Libraries
 #include "../body/BoundingVolume.h"

src/collision/NoBroadPhaseAlgorithm.h

@@ -22,8 +22,8 @@
 * THE SOFTWARE.                                                                 *
 ********************************************************************************/
 
-#ifndef NOBROADPHASEALGORITHM_H
+#ifndef NO_BROAD_PHASE_ALGORITHM_H
-#define NOBROADPHASEALGORITHM_H
+#define NO_BROAD_PHASE_ALGORITHM_H
 
 // Libraries
 #include "BroadPhaseAlgorithm.h"

src/collision/SAPAlgorithm.h

@@ -22,8 +22,8 @@
 * THE SOFTWARE.                                                                 *
 ********************************************************************************/
 
-#ifndef SAPALGORITHM_H
+#ifndef SAP_ALGORITHM_H
-#define SAPALGORITHM_H
+#define SAP_ALGORITHM_H
 
 // Libraries
 #include "BroadPhaseAlgorithm.h"

src/collision/SATAlgorithm.h

@@ -22,8 +22,8 @@
 * THE SOFTWARE.                                                                 *
 ********************************************************************************/
 
-#ifndef SATALGORITHM_H
+#ifndef SAT_ALGORITHM_H
-#define SATALGORITHM_H
+#define SAT_ALGORITHM_H
 
 // Libraries
 #include "NarrowPhaseAlgorithm.h"

src/engine/ConstraintSolver.h

@@ -22,8 +22,8 @@
 * THE SOFTWARE.                                                                 *
 ********************************************************************************/
 
-#ifndef CONSTRAINTSOLVER_H
+#ifndef CONSTRAINT_SOLVER_H
-#define CONSTRAINTSOLVER_H
+#define CONSTRAINT_SOLVER_H
 
 // Libraries
 #include "../constants.h"

src/engine/ContactCache.h

@@ -22,8 +22,8 @@
 * THE SOFTWARE.                                                                 *
 ********************************************************************************/
 
-#ifndef CONTACTCACHE_H
+#ifndef CONTACT_CACHE_H
-#define	CONTACTCACHE_H
+#define	CONTACT_CACHE_H
 
 // Libraries
 #include <map>

src/engine/ContactCachingInfo.h

@@ -22,8 +22,8 @@
 * THE SOFTWARE.                                                                 *
 ********************************************************************************/
 
-#ifndef CONTACTCACHINGINFO_H
+#ifndef CONTACT_CACHING_INFO_H
-#define	CONTACTCACHINGINFO_H
+#define	CONTACT_CACHING_INFO_H
 
 // Libraries
 #include "../body/OBB.h"

src/engine/PhysicsEngine.h

@@ -22,8 +22,8 @@
 * THE SOFTWARE.                                                                 *
 ********************************************************************************/
 
-#ifndef PHYSICSENGINE_H
+#ifndef PHYSICS_ENGINE_H
-#define PHYSICSENGINE_H
+#define PHYSICS_ENGINE_H
 
 // Libraries
 #include "PhysicsWorld.h"

src/engine/PhysicsWorld.h

@@ -22,8 +22,8 @@
 * THE SOFTWARE.                                                                 *
 ********************************************************************************/
 
-#ifndef PHYSICSWORLD_H
+#ifndef PHYSICS_WORLD_H
-#define PHYSICSWORLD_H
+#define PHYSICS_WORLD_H
 
 // Libraries
 #include <vector>

src/mathematics/Quaternion.cpp

@@ -207,7 +207,7 @@ Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& qu
 
     // Because of precision, if cos(theta) is nearly 1, therefore theta is nearly 0 and we can write
     // sin((1-t)*theta) as (1-t) and sin(t*theta) as t
-    if(1-cosineTheta < EPSILON) {
+    if(1-cosineTheta < EPSILON_TEST) {
         return quaternion1 * (1.0-t) + quaternion2 * (t * invert);
     }
 

src/mathematics/Vector3D.h

@@ -70,6 +70,9 @@ class Vector3D {
         Vector3D getOneOrthogonalVector() const;                // Return one unit orthogonal vectors of the current vector
         double dot(const Vector3D& vector) const;               // Dot product of two vectors
         Vector3D cross(const Vector3D& vector) const;           // Cross product of two vectors
+        Vector3D getAbsoluteVector() const;                     // Return the corresponding absolute value vector
+        int getMinAxis() const;                                 // Return the axis with the minimal value
+        int getMaxAxis() const;                                 // Return the axis with the maximal value
         bool isParallelWith(const Vector3D& vector) const;      // Return true if two vectors are parallel
 
         // --- Overloaded operators --- //
@@ -162,12 +165,27 @@ inline Vector3D Vector3D::cross(const Vector3D& vector) const {
     return Vector3D(y * vector.z - z * vector.y, z * vector.x - x * vector.z , x * vector.y - y * vector.x);
 }
 
+// Return the corresponding absolute value vector
+inline Vector3D Vector3D::getAbsoluteVector() const {
+    return Vector3D(std::abs(x), std::abs(y), std::abs(z));
+}       
+
 // Return true if two vectors are parallel
 inline bool Vector3D::isParallelWith(const Vector3D& vector) const {
     double scalarProd = this->dot(vector);
     return approxEqual(std::abs(scalarProd), length() * vector.length());
 }
 
+// Return the axis with the minimal value
+inline int Vector3D::getMinAxis() const {
+    return (x < y ? (x < z ? 0 : 2) : (y < z ? 1 : 2));
+}
+
+// Return the axis with the maximal value
+inline int Vector3D::getMaxAxis() const {
+    return (x < y ? (y < z ? 2 : 1) : (x < z ? 2 : 0));
+}
+
 // Return true if the vector is unit and false otherwise
 inline bool Vector3D::isUnit() const {
     return approxEqual(x*x+y*y+z*z, 1.0);

src/mathematics/lcp/LCPProjectedGaussSeidel.h

@@ -22,8 +22,8 @@
 * THE SOFTWARE.                                                                 *
 ********************************************************************************/
 
-#ifndef LCPPROJECTEDGAUSSSEIDEL_H
+#ifndef LCP_PROJECTED_GAUSS_SEIDEL_H
-#define LCPPROJECTEDGAUSSSEIDEL_H
+#define LCP_PROJECTED_GAUSS_SEIDEL_H
 
 // Libraries
 #include "LCPSolver.h"

src/mathematics/lcp/LCPSolver.h

@@ -22,8 +22,8 @@
 * THE SOFTWARE.                                                                 *
 ********************************************************************************/
 
-#ifndef LCPSOLVER_H
+#ifndef LCP_SOLVER_H
-#define LCPSOLVER_H
+#define LCP_SOLVER_H
 
 // Libraries
 #include <vector>

src/mathematics/mathematics.h

@@ -148,7 +148,7 @@ inline reactphysics3d::Vector3D computeNonParallelSegmentsIntersection(const rea
 
     // If the two closest point aren't very close, there is no intersection between the segments
     reactphysics3d::Vector3D d = point2 - point1;
-    assert(d.length() <= EPSILON);
+    assert(d.length() <= EPSILON_TEST);
 
     // They are very close so we return the intersection point (halfway between "point1" and "point2"
     return 0.5 * (point1 + point2);

src/mathematics/mathematics_functions.h

@@ -37,7 +37,7 @@ namespace reactphysics3d {
 // We test if two numbers a and b are such that (a-b) are in [-EPSILON; EPSILON]
 inline bool approxEqual(double a, double b) {
     double difference = a - b;
-    return (difference < EPSILON && difference > -EPSILON);
+    return (difference < EPSILON_TEST && difference > -EPSILON_TEST);
 }
 
 } // End of ReactPhysics3D namespace