implementation of GJK and EPA collision detection algorithm continued

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

src/body/BoundingSphere.cpp

@@ -1,7 +1,7 @@
 
 /********************************************************************************
 * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
-* Copyright (c) 2010 Daniel Chappuis                                            *
+* Copyright (c) 2011 Daniel Chappuis                                            *
 *********************************************************************************
 *                                                                               *
 * Permission is hereby granted, free of charge, to any person obtaining a copy  *

src/body/BoundingSphere.h

@@ -1,6 +1,6 @@
 /********************************************************************************
 * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
-* Copyright (c) 2010 Daniel Chappuis                                            *
+* Copyright (c) 2011 Daniel Chappuis                                            *
 *********************************************************************************
 *                                                                               *
 * Permission is hereby granted, free of charge, to any person obtaining a copy  *
@@ -46,14 +46,14 @@ class BoundingSphere : public NarrowBoundingVolume {
         BoundingSphere(const Vector3D& center, double radius);      // Constructor
         virtual ~BoundingSphere();                                  // Destructor
 
-        Vector3D getCenter() const;                                         // Return the center point of the sphere
+        Vector3D getCenter() const;                                                             // Return the center point of the sphere
-        void setCenter(const Vector3D& center);                             // Set the center point of the sphere
+        void setCenter(const Vector3D& center);                                                 // Set the center point of the sphere
-        double getRadius() const;                                           // Return the radius of the sphere
+        double getRadius() const;                                                               // Return the radius of the sphere
-        void setRadius(double radius);                                      // Set the radius of the sphere
+        void setRadius(double radius);                                                          // Set the radius of the sphere
         virtual void update(const Vector3D& newCenter,
-                            const Quaternion& rotationQuaternion);          // Update the sphere orientation according to a new orientation of the rigid body
+                            const Quaternion& rotationQuaternion);                              // Update the sphere orientation according to a new orientation of the rigid body
-        virtual AABB* computeAABB() const;                                  // Return the corresponding AABB
+        virtual AABB* computeAABB() const;                                                      // Return the corresponding AABB
-        virtual Vector3D getSupportPoint(const Vector3D& direction) const;  // Return a support point in a given direction
+        virtual Vector3D getSupportPoint(const Vector3D& direction, double margin=0.0) const;   // Return a support point in a given direction
 
 #ifdef VISUAL_DEBUG
             virtual void draw() const;                              // Draw the sphere (only for testing purpose)
@@ -87,11 +87,12 @@ inline void BoundingSphere::update(const Vector3D& newCenter, const Quaternion&
 }
 
 // Return a support point in a given direction
-inline Vector3D BoundingSphere::getSupportPoint(const Vector3D& direction) const {
+inline Vector3D BoundingSphere::getSupportPoint(const Vector3D& direction, double margin) const {
     assert(direction.length() > 0.0);
+    assert(margin >= 0.0);
 
     // Return the support point of the sphere in the given direction
-    return center + radius * direction.getUnit();
+    return center + (radius + margin) * direction.getUnit();
 }
 
 }; // End of the ReactPhysics3D namespace

src/body/NarrowBoundingVolume.h

@@ -48,8 +48,8 @@ class NarrowBoundingVolume : public BoundingVolume {
         NarrowBoundingVolume();                 // Constructor
         virtual ~NarrowBoundingVolume();        // Destructor
 
-        virtual AABB* computeAABB() const=0;                                    // Return the corresponding AABB
+        virtual AABB* computeAABB() const=0;                                                        // Return the corresponding AABB
-        virtual Vector3D getSupportPoint(const Vector3D& direction) const=0;    // Return a support point in a given direction
+        virtual Vector3D getSupportPoint(const Vector3D& direction, double margin=0.0) const=0;     // Return a support point in a given direction
 };
 
 }

src/body/OBB.cpp

@@ -131,7 +131,7 @@ vector<Vector3D> OBB::getExtremeVertices(const Vector3D& directionAxis) const {
 
     // Check if the given axis is parallel to an axis on the OBB
     if (axis[0].isParallelWith(directionAxis)) {
-        if (axis[0].scalarProduct(directionAxis) >= 0) {    // If both axis are in the same direction
+        if (axis[0].dot(directionAxis) >= 0) {    // If both axis are in the same direction
             extremeVertices = getFace(0);           // The extreme is the face 0
         }
         else {
@@ -139,7 +139,7 @@ vector<Vector3D> OBB::getExtremeVertices(const Vector3D& directionAxis) const {
         }
     }
     else if(axis[1].isParallelWith(directionAxis)) {
-        if (axis[1].scalarProduct(directionAxis) >= 0) {    // If both axis are in the same direction
+        if (axis[1].dot(directionAxis) >= 0) {    // If both axis are in the same direction
            extremeVertices = getFace(2);            // The extreme is the face 2
         }
         else {
@@ -148,7 +148,7 @@ vector<Vector3D> OBB::getExtremeVertices(const Vector3D& directionAxis) const {
 
     }
     else if(axis[2].isParallelWith(directionAxis)) {
-        if (axis[2].scalarProduct(directionAxis) >= 0) {     // If both axis are in the same direction
+        if (axis[2].dot(directionAxis) >= 0) {     // If both axis are in the same direction
           extremeVertices = getFace(4);             // The extreme is the face 4
         }
         else {
@@ -163,7 +163,7 @@ vector<Vector3D> OBB::getExtremeVertices(const Vector3D& directionAxis) const {
             Vector3D vertex = getVertex(i);
 
             // Compute the projection length of the current vertex onto the projection axis
-            double projectionLength = directionAxis.scalarProduct(vertex-center) / directionAxis.length();
+            double projectionLength = directionAxis.dot(vertex-center) / directionAxis.length();
 
             // If we found a bigger projection length
             if (projectionLength > maxProjectionLength + EPSILON) {

src/body/OBB.h

@@ -66,7 +66,7 @@ class OBB : public NarrowBoundingVolume {
         virtual std::vector<Vector3D> getExtremeVertices(const Vector3D& axis) const;                       // Return all the vertices that are projected at the extreme of the projection of the bouding volume on the axis
         virtual void update(const Vector3D& newCenter, const Quaternion& rotationQuaternion);               // Update the oriented bounding box orientation according to a new orientation of the rigid body
         virtual AABB* computeAABB() const;                                                                  // Return the corresponding AABB
-        virtual Vector3D getSupportPoint(const Vector3D& direction) const;                                  // Return a support point in a given direction
+        virtual Vector3D getSupportPoint(const Vector3D& direction, double margin=0.0) const;               // Return a support point in a given direction
 
 #ifdef VISUAL_DEBUG
             virtual void draw() const;                                                                      // Draw the OBB (only for testing purpose)
@@ -228,7 +228,10 @@ inline void OBB::update(const Vector3D& newCenter, const Quaternion& rotationQua
 }
 
 // Return a support point in a given direction
-inline Vector3D OBB::getSupportPoint(const Vector3D& direction) const {
+inline Vector3D OBB::getSupportPoint(const Vector3D& direction, double margin) const {
+    assert(direction.length() > 0.0);
+    assert(margin >= 0.0);
+    
     // TODO : Implement this method
     assert(false);
     return Vector3D();

src/collision/EPA/EPAAlgorithm.h

@@ -27,9 +27,9 @@
 
 // Libraries
 #include "../GJK/Simplex.h"
-#include "../body/NarrowBoundingVolume.h"
+#include "../../body/NarrowBoundingVolume.h"
-#include "ContactInfo.h"
+#include "../ContactInfo.h"
-#include "../mathematics/mathematics.h"
+#include "../../mathematics/mathematics.h"
 
 // ReactPhysics3D namespace
 namespace reactphysics3d {

src/collision/EPA/TriangleEPA.cpp

@@ -71,7 +71,7 @@ bool TriangleEPA::computeClosestPoint(const Vector3D* vertices) {
     // If the determinant is positive
     if (det > 0.0) {
         // Compute the closest point v
-        closestPoint = p0 + (lambda1 * v1 * lambda2 * v2) / det;
+        closestPoint = p0 + 1.0 / det * (lambda1 * v1 + lambda2 * v2);
 
         // Compute the square distance of closest point to the origin
         distSquare = closestPoint.dot(closestPoint);

src/collision/GJK/GJKAlgorithm.cpp

@@ -26,8 +26,8 @@
 // Libraries
 #include "GJKAlgorithm.h"
 #include "Simplex.h"
-#include "../constraint/Contact.h"
+#include "../../constraint/Contact.h"
-#include "../constants.h"
+#include "../../constants.h"
 #include <algorithm>
 #include <cmath>
 #include <cfloat>

src/collision/GJK/GJKAlgorithm.h

@@ -26,10 +26,10 @@
 #define GJKALGORITHM_H
 
 // Libraries
-#include "NarrowPhaseAlgorithm.h"
+#include "../NarrowPhaseAlgorithm.h"
-#include "ContactInfo.h"
+#include "../ContactInfo.h"
-#include "../body/NarrowBoundingVolume.h"
+#include "../../body/NarrowBoundingVolume.h"
-#include "EPAAlgorithm.h"
+#include "../EPA/EPAAlgorithm.h"
 
 
 // ReactPhysics3D namespace

src/collision/GJK/Simplex.h

@@ -26,7 +26,7 @@
 #define	SIMPLEX_H
 
 // Libraries
-#include "../mathematics/mathematics.h"
+#include "../../mathematics/mathematics.h"
 #include <vector>
 
 // ReactPhysics3D namespace

src/constraint/Contact.cpp

@@ -74,8 +74,8 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
 
         r1 = points[i] - body1Position;
         r2 = points[i] - body2Position;
-        r1CrossN = r1.crossProduct(normal);
+        r1CrossN = r1.cross(normal);
-        r2CrossN = r2.crossProduct(normal);
+        r2CrossN = r2.cross(normal);
 
         // Compute the jacobian matrix for the body 1 for the contact constraint
         //J_sp[currentIndex][0].changeSize(1, 6);
@@ -98,10 +98,10 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
         currentIndex++;
 
         // Compute the jacobian matrix for the body 1 for the first friction constraint
-        r1CrossU1 = r1.crossProduct(frictionVectors[0]);
+        r1CrossU1 = r1.cross(frictionVectors[0]);
-        r2CrossU1 = r2.crossProduct(frictionVectors[0]);
+        r2CrossU1 = r2.cross(frictionVectors[0]);
-        r1CrossU2 = r1.crossProduct(frictionVectors[1]);
+        r1CrossU2 = r1.cross(frictionVectors[1]);
-        r2CrossU2 = r2.crossProduct(frictionVectors[1]);
+        r2CrossU2 = r2.cross(frictionVectors[1]);
         //J_sp[currentIndex][0].changeSize(1, 6);
         J_sp[currentIndex][0].setValue(0, -frictionVectors[0].getX());
         J_sp[currentIndex][0].setValue(1, -frictionVectors[0].getY());
@@ -193,7 +193,7 @@ void Contact::computeErrorValue(int noConstraint, Vector& errorValues) const {
     Vector3D velocity1 = rigidBody1->getLinearVelocity();
     Vector3D velocity2 = rigidBody2->getLinearVelocity();
     double restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution();
-    double errorValue = restitutionCoeff * (normal.scalarProduct(velocity1) - normal.scalarProduct(velocity2)) + PENETRATION_FACTOR * penetrationDepth;
+    double errorValue = restitutionCoeff * (normal.dot(velocity1) - normal.dot(velocity2)) + PENETRATION_FACTOR * penetrationDepth;
 
     // Assign the error value to the vector of error values
     for (int i=0; i<nbPoints; i++) {

src/constraint/Contact.h

@@ -87,7 +87,7 @@ inline void Contact::computeFrictionVectors() {
     frictionVectors.push_back(vector1);
 
     // Compute the second orthogonal vector using the cross product
-    frictionVectors.push_back(normal.crossProduct(vector1));
+    frictionVectors.push_back(normal.cross(vector1));
 }
 
 // Return the normal vector of the contact

src/mathematics/Quaternion.cpp

@@ -197,7 +197,7 @@ Quaternion Quaternion::slerp(const Quaternion& quaternion1, const Quaternion& qu
     double invert = 1.0;
 
     // Compute cos(theta) using the quaternion scalar product
-    double cosineTheta = quaternion1.scalarProduct(quaternion2);
+    double cosineTheta = quaternion1.dot(quaternion2);
 
     // Take care of the sign of cosineTheta
     if (cosineTheta < 0.0) {

src/mathematics/Quaternion.h

@@ -68,7 +68,7 @@ class Quaternion {
         Quaternion getConjugate() const;                                      // Return the conjugate quaternion
         Quaternion getInverse() const throw (MathematicsException);           // Return the inverse of the quaternion
         Matrix3x3 getMatrix() const;                                          // Return the orientation matrix corresponding to this quaternion
-        double scalarProduct(const Quaternion& quaternion) const;             // Scalar product between two quaternions
+        double dot(const Quaternion& quaternion) const;                       // Dot product between two quaternions
         void getRotationAngleAxis(double& angle, Vector3D& axis) const;       // Compute the rotation angle (in radians) and the axis
         static Quaternion slerp(const Quaternion& quaternion1,
                                 const Quaternion& quaternion2, double t);     // Compute the spherical linear interpolation between two quaternions
@@ -174,7 +174,7 @@ inline Quaternion Quaternion::getInverse() const throw(MathematicsException) {
 }
 
 // Scalar product between two quaternions
-inline double Quaternion::scalarProduct(const Quaternion& quaternion) const {
+inline double Quaternion::dot(const Quaternion& quaternion) const {
     return (x*quaternion.x + y*quaternion.y + z*quaternion.z + w*quaternion.w);
 }
 
@@ -199,7 +199,7 @@ inline Quaternion Quaternion::operator*(double nb) const {
 // Overloaded operator for the multiplication of two quaternions
 inline Quaternion Quaternion::operator*(const Quaternion& quaternion) const {
     // Return the result of the multiplication
-    return Quaternion(w*quaternion.w - vectorV().scalarProduct(quaternion.vectorV()), w*quaternion.vectorV()+quaternion.w*vectorV() + vectorV().crossProduct(quaternion.vectorV()));
+    return Quaternion(w*quaternion.w - vectorV().dot(quaternion.vectorV()), w*quaternion.vectorV()+quaternion.w*vectorV() + vectorV().cross(quaternion.vectorV()));
 }
 
 // Overloaded operator for the assignment

src/mathematics/Vector3D.h

@@ -67,9 +67,9 @@ class Vector3D {
         bool isUnit() const;                                    // Return true if the vector is unit and false otherwise
         bool isZero() const;                                    // Return true if the current vector is the zero vector
         Vector3D getOpposite() const;                           // Return the vector in the opposite direction
-        Vector3D getOneOrthogonalVector() const;               // Return one unit orthogonal vectors of the current vector
+        Vector3D getOneOrthogonalVector() const;                // Return one unit orthogonal vectors of the current vector
-        double scalarProduct(const Vector3D& vector) const;     // Scalar product of two vectors
+        double dot(const Vector3D& vector) const;               // Dot product of two vectors
-        Vector3D crossProduct(const Vector3D& vector) const;    // Cross product of two vectors
+        Vector3D cross(const Vector3D& vector) const;           // Cross product of two vectors
         bool isParallelWith(const Vector3D& vector) const;      // Return true if two vectors are parallel
 
         // --- Overloaded operators --- //
@@ -151,20 +151,20 @@ inline Vector3D Vector3D::getOpposite() const {
 }
 
 // Scalar product of two vectors (inline)
-inline double Vector3D::scalarProduct(const Vector3D& vector) const {
+inline double Vector3D::dot(const Vector3D& vector) const {
     // Compute and return the result of the scalar product
     return (x * vector.x + y * vector.y + z * vector.z);
 }
 
 // Cross product of two vectors (inline)
-inline Vector3D Vector3D::crossProduct(const Vector3D& vector) const {
+inline Vector3D Vector3D::cross(const Vector3D& vector) const {
     // Compute and return the cross product
     return Vector3D(y * vector.z - z * vector.y, z * vector.x - x * vector.z , x * vector.y - y * vector.x);
 }
 
 // Return true if two vectors are parallel
 inline bool Vector3D::isParallelWith(const Vector3D& vector) const {
-    double scalarProd = this->scalarProduct(vector);
+    double scalarProd = this->dot(vector);
     return approxEqual(std::abs(scalarProd), length() * vector.length());
 }
 

src/mathematics/mathematics.h

@@ -65,11 +65,11 @@ inline void closestPointsBetweenTwoLines(const reactphysics3d::Vector3D& point1,
                                          const reactphysics3d::Vector3D& d2, double* alpha, double* beta) {
 
     reactphysics3d::Vector3D r = point1 - point2;
-    double a = d1.scalarProduct(d1);
+    double a = d1.dot(d1);
-    double b = d1.scalarProduct(d2);
+    double b = d1.dot(d2);
-    double c = d1.scalarProduct(r);
+    double c = d1.dot(r);
-    double e = d2.scalarProduct(d2);
+    double e = d2.dot(d2);
-    double f = d2.scalarProduct(r);
+    double f = d2.dot(r);
     double d = a*e-b*b;
 
     // The two lines must not be parallel
@@ -183,7 +183,7 @@ inline std::vector<reactphysics3d::Vector3D> projectPointsOntoPlane(const std::v
     // For each point of the set
     for (unsigned int i=0; i<points.size(); ++i) {
         // Compute the projection of the point onto the plane
-        projectedPoints.push_back(points[i] - (n * (points[i] - A).scalarProduct(n)));
+        projectedPoints.push_back(points[i] - (n * (points[i] - A).dot(n)));
 
     }
 
@@ -195,13 +195,13 @@ inline std::vector<reactphysics3d::Vector3D> projectPointsOntoPlane(const std::v
 // Compute the distance between a point "P" and a line (given by a point "A" and a vector "v")
 inline double computeDistanceBetweenPointAndLine(const reactphysics3d::Vector3D& P, const reactphysics3d::Vector3D& A, const reactphysics3d::Vector3D& v) {
     assert(v.length() != 0);
-    return ((P-A).crossProduct(v).length() / (v.length()));
+    return ((P-A).cross(v).length() / (v.length()));
 }
 
 
 // Compute the orthogonal projection of a point "P" on a line (given by a point "A" and a vector "v")
 inline reactphysics3d::Vector3D computeOrthogonalProjectionOfPointOntoALine(const reactphysics3d::Vector3D& P, const reactphysics3d::Vector3D& A, const reactphysics3d::Vector3D& v) {
-    return (A + ((P-A).scalarProduct(v) / (v.scalarProduct(v))) * v);
+    return (A + ((P-A).dot(v) / (v.dot(v))) * v);
 }
 
 // Given a point P and 4 points that form a rectangle (point P and the 4 points have to be on the same plane) this method computes
@@ -281,8 +281,8 @@ inline void computeParallelSegmentsIntersection(const reactphysics3d::Vector3D&
     assert(d1.isParallelWith(d2));
 
     // Compute the projection of the two points of the second segment onto the vector of segment 1
-    double projSeg2PointA = d1.getUnit().scalarProduct(seg2PointA - seg1PointA);
+    double projSeg2PointA = d1.getUnit().dot(seg2PointA - seg1PointA);
-    double projSeg2PointB = d1.getUnit().scalarProduct(seg2PointB - seg1PointA);
+    double projSeg2PointB = d1.getUnit().dot(seg2PointB - seg1PointA);
 
     // The projections intervals should intersect
     assert(!(projSeg2PointA < 0.0 && projSeg2PointB < 0.0));
@@ -343,9 +343,9 @@ inline std::vector<reactphysics3d::Vector3D> clipSegmentWithRectangleInPlane(con
         reactphysics3d::Vector3D planeNormal = clipRectangle[(i+2) % 4] - clipRectangle[(i+1) % 4];
 
         // If the point P is inside the clip plane
-        if (planeNormal.scalarProduct(P-A) >= 0.0 - epsilon) {
+        if (planeNormal.dot(P-A) >= 0.0 - epsilon) {
             // If the point S is inside the clip plane
-            if (planeNormal.scalarProduct(S-A) >= 0.0 - epsilon) {
+            if (planeNormal.dot(S-A) >= 0.0 - epsilon) {
                 outputSegment.push_back(P);
                 outputSegment.push_back(S);
             }
@@ -357,7 +357,7 @@ inline std::vector<reactphysics3d::Vector3D> clipSegmentWithRectangleInPlane(con
                 outputSegment.push_back(intersectPoint);
             }
         }
-        else if (planeNormal.scalarProduct(S-A) > 0.0 - epsilon) {    // P is outside and S is inside the clip plane
+        else if (planeNormal.dot(S-A) > 0.0 - epsilon) {    // P is outside and S is inside the clip plane
                 // Compute the intersection point between the segment SP and the clip plane
                 reactphysics3d::Vector3D intersectPoint = computeLinesIntersection(S, P-S, A, B-A);
 
@@ -398,10 +398,10 @@ inline std::vector<reactphysics3d::Vector3D> clipPolygonWithRectangleInPlane(con
             reactphysics3d::Vector3D P = inputPolygon[(j+1) % inputPolygon.size()];
 
             // If the point P is inside the clip plane
-            double test = planeNormal.scalarProduct(P-A);
+            double test = planeNormal.dot(P-A);
-            if (planeNormal.scalarProduct(P-A) >= 0.0 - epsilon) {
+            if (planeNormal.dot(P-A) >= 0.0 - epsilon) {
                 // If the point S is also inside the clip plane
-                if (planeNormal.scalarProduct(S-A) >= 0.0 - epsilon) {
+                if (planeNormal.dot(S-A) >= 0.0 - epsilon) {
                     outputPolygon.push_back(P);
                 }
                 else {  // If the point S is outside the clip plane
@@ -412,7 +412,7 @@ inline std::vector<reactphysics3d::Vector3D> clipPolygonWithRectangleInPlane(con
                     outputPolygon.push_back(P);
                 }
             }
-            else if (planeNormal.scalarProduct(S-A) > 0.0) {
+            else if (planeNormal.dot(S-A) > 0.0) {
                 // Compute the intersection point between the segment SP and the clip plane
                 reactphysics3d::Vector3D intersectPoint = computeLinesIntersection(S, P-S, A, B-A);
 
@@ -431,13 +431,13 @@ inline std::vector<reactphysics3d::Vector3D> clipPolygonWithRectangleInPlane(con
 // the lineVector must not be orthogonal to the planeNormal.
 inline reactphysics3d::Vector3D intersectLineWithPlane(const reactphysics3d::Vector3D& linePoint, const reactphysics3d::Vector3D& lineVector,
                                                        const reactphysics3d::Vector3D& planePoint, const reactphysics3d::Vector3D& planeNormal) {
-    assert(!approxEqual(lineVector.scalarProduct(planeNormal), 0.0));
+    assert(!approxEqual(lineVector.dot(planeNormal), 0.0));
 
     // The plane is represented by the equation planeNormal dot X = d where X is a point of the plane
-    double d  = planeNormal.scalarProduct(planePoint);
+    double d  = planeNormal.dot(planePoint);
 
     // Compute the parameter t
-    double t = (d - planeNormal.scalarProduct(linePoint)) / planeNormal.scalarProduct(lineVector);
+    double t = (d - planeNormal.dot(linePoint)) / planeNormal.dot(lineVector);
 
     // Compute the intersection point
     return linePoint + lineVector * t;