reactphysics3d/src/collision/GJK/GJKAlgorithm.cpp

/********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/      *
* Copyright (c) 2011 Daniel Chappuis                                            *
*********************************************************************************
*                                                                               *
* Permission is hereby granted, free of charge, to any person obtaining a copy  *
* of this software and associated documentation files (the "Software"), to deal *
* in the Software without restriction, including without limitation the rights  *
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell     *
* copies of the Software, and to permit persons to whom the Software is         *
* furnished to do so, subject to the following conditions:                      *
*                                                                               *
* The above copyright notice and this permission notice shall be included in    *
* all copies or substantial portions of the Software.                           *
*                                                                               *
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR    *
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,      *
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE   *
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER        *
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, *
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN     *
* THE SOFTWARE.                                                                 *
********************************************************************************/

// Libraries
#include "GJKAlgorithm.h"
#include "Simplex.h"
#include "../../constraint/Contact.h"
#include "../../constants.h"
#include <algorithm>
#include <cmath>
#include <cfloat>
#include <cassert>


// We want to use the ReactPhysics3D namespace
using namespace reactphysics3d;

// Constructor
GJKAlgorithm::GJKAlgorithm(CollisionDetection& collisionDetection)
             :NarrowPhaseAlgorithm(collisionDetection) {
    
}

// Destructor
GJKAlgorithm::~GJKAlgorithm() {

}

// Return true and compute a contact info if the two bounding volume collide.
// This method implements the Hybrid Technique for computing the penetration depth by
// running the GJK algorithm on original objects (without margin).
// If the objects don't intersect, this method returns false. If they intersect
// only in the margins, the method compute the penetration depth and contact points
// (of enlarged objects). If the original objects (without margin) intersect, we
// call the computePenetrationDepthForEnlargedObjects() method that run the GJK
// algorithm on the enlarged object to obtain a simplex polytope that contains the
// origin, they we give that simplex polytope to the EPA algorithm which will compute
// the correct penetration depth and contact points between the enlarged objects.
bool GJKAlgorithm::testCollision(const Shape* shape1, const Transform& transform1,
                                 const Shape* shape2,  const Transform& transform2,
		                         ContactInfo*& contactInfo) {

    assert(shape1 != shape2);
    
    Vector3 suppA;             // Support point of object A
    Vector3 suppB;             // Support point of object B
    Vector3 w;                 // Support point of Minkowski difference A-B
    Vector3 pA;                // Closest point of object A
    Vector3 pB;                // Closest point of object B
    double vDotw;
    double prevDistSquare;
    Body* const body1 = shape1->getBodyPointer();
    Body* const body2 = shape2->getBodyPointer();

    // Transform a point from body space of shape 2 to body space of shape 1 (the GJK algorithm is done in body space of shape 1)
    Transform shape2ToShape1 = transform1.inverse() * transform2;

    // Matrix that transform a direction from body space of shape 1 into body space of shape 2
    Matrix3x3 rotateToShape2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix();

    // Initialize the margin (sum of margins of both objects)
    double margin = 2 * OBJECT_MARGIN;
    double marginSquare = margin * margin;
    assert(margin > 0.0);

    // Create a simplex set
    Simplex simplex;

    // Get the previous point V (last cached separating axis)
	OverlappingPair* overlappingPair = collisionDetection.getOverlappingPair(body1->getID(), body2->getID());
	Vector3 v = (overlappingPair) ? overlappingPair->getCachedSeparatingAxis() : Vector3(1.0, 1.0, 1.0);
			
    // Initialize the upper bound for the square distance
    double distSquare = DBL_MAX;
    
    do {
        // Compute the support points for original objects (without margins) A and B
        suppA = shape1->getLocalSupportPoint(-v);
        suppB = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v);

        // Compute the support point for the Minkowski difference A-B
        w = suppA - suppB;
        
        vDotw = v.dot(w);
        
        // If the enlarge objects (with margins) do not intersect
        if (vDotw > 0.0 && vDotw * vDotw > distSquare * marginSquare) {
            // No intersection, we return false
            return false;
        }

        // If the objects intersect only in the margins
        if (simplex.isPointInSimplex(w) || distSquare - vDotw <= distSquare * REL_ERROR_SQUARE) {
            // Compute the closet points of both objects (without the margins)
            simplex.computeClosestPointsOfAandB(pA, pB);

            // Project those two points on the margins to have the closest points of both
            // object with the margins
            double dist = sqrt(distSquare);
            assert(dist > 0.0);
            pA = (pA - (OBJECT_MARGIN / dist) * v);
            pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);

            // Compute the contact info
            Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
            double penetrationDepth = margin - dist;
            contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB, transform1, transform2);

            // There is an intersection, therefore we return true
            return true;
        }

        // Add the new support point to the simplex
        simplex.addPoint(w, suppA, suppB);

        // If the simplex is affinely dependent
        if (simplex.isAffinelyDependent()) {
            // Compute the closet points of both objects (without the margins)
            simplex.computeClosestPointsOfAandB(pA, pB);

            // Project those two points on the margins to have the closest points of both
            // object with the margins
            double dist = sqrt(distSquare);
            assert(dist > 0.0);
            pA = (pA - (OBJECT_MARGIN / dist) * v);
            pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);

            // Compute the contact info
            Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
            double penetrationDepth = margin - dist;
            contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB, transform1, transform2);

            // There is an intersection, therefore we return true
            return true;
        }

        // Compute the point of the simplex closest to the origin
        // If the computation of the closest point fail
        if (!simplex.computeClosestPoint(v)) {
            // Compute the closet points of both objects (without the margins)
            simplex.computeClosestPointsOfAandB(pA, pB);

            // Project those two points on the margins to have the closest points of both
            // object with the margins
            double dist = sqrt(distSquare);
            assert(dist > 0.0);
            pA = (pA - (OBJECT_MARGIN / dist) * v);
            pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);

            // Compute the contact info
            Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
            double penetrationDepth = margin - dist;
            contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB, transform1, transform2);

            // There is an intersection, therefore we return true
            return true;
        }

        // Store and update the squared distance of the closest point
        prevDistSquare = distSquare;
        distSquare = v.lengthSquare();

        // If the distance to the closest point doesn't improve a lot
        if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
            simplex.backupClosestPointInSimplex(v);
            
            // Get the new squared distance
            distSquare = v.lengthSquare();

            // Compute the closet points of both objects (without the margins)
            simplex.computeClosestPointsOfAandB(pA, pB);

            // Project those two points on the margins to have the closest points of both
            // object with the margins
            double dist = sqrt(distSquare);
            assert(dist > 0.0);
            pA = (pA - (OBJECT_MARGIN / dist) * v);
            pB = shape2ToShape1.inverse() * (pB + (OBJECT_MARGIN / dist) * v);

            // Compute the contact info
            Vector3 normal = transform1.getOrientation().getMatrix() * (-v.getUnit());
            double penetrationDepth = margin - dist;
            contactInfo = new ContactInfo(body1, body2, normal, penetrationDepth, pA, pB, transform1, transform2);

            // There is an intersection, therefore we return true
            return true;
        }
    } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint());

    // The objects (without margins) intersect. Therefore, we run the GJK algorithm again but on the
    // enlarged objects to compute a simplex polytope that contains the origin. Then, we give that simplex
    // polytope to the EPA algorithm to compute the correct penetration depth and contact points between
    // the enlarged objects.
    return computePenetrationDepthForEnlargedObjects(shape1, transform1, shape2, transform2, contactInfo, v);
}

// This method runs the GJK algorithm on the two enlarged objects (with margin)
// to compute a simplex polytope that contains the origin. The two objects are
// assumed to intersect in the original objects (without margin). Therefore such
// a polytope must exist. Then, we give that polytope to the EPA algorithm to
// compute the correct penetration depth and contact points of the enlarged objects.
bool GJKAlgorithm::computePenetrationDepthForEnlargedObjects(const Shape* const shape1, const Transform& transform1,
                                                             const Shape* const shape2, const Transform& transform2,
                                                             ContactInfo*& contactInfo, Vector3& v) {
    Simplex simplex;
    Vector3 suppA;
    Vector3 suppB;
    Vector3 w;
    double vDotw;
    double distSquare = DBL_MAX;
    double prevDistSquare;

    // Transform a point from body space of shape 2 to body space of shape 1 (the GJK algorithm is done in body space of shape 1)
    Transform shape2ToShape1 = transform1.inverse() * transform2;

    // Matrix that transform a direction from body space of shape 1 into body space of shape 2
    Matrix3x3 rotateToShape2 = transform2.getOrientation().getMatrix().getTranspose() * transform1.getOrientation().getMatrix();
    
    do {
        // Compute the support points for the enlarged object A and B
        suppA = shape1->getLocalSupportPoint(-v, OBJECT_MARGIN);
        suppB = shape2ToShape1 * shape2->getLocalSupportPoint(rotateToShape2 * v, OBJECT_MARGIN);

        // Compute the support point for the Minkowski difference A-B
        w = suppA - suppB;

        vDotw = v.dot(w);

        // If the enlarge objects do not intersect
        if (vDotw > 0.0) {
            // No intersection, we return false
            return false;
        }

        // Add the new support point to the simplex
        simplex.addPoint(w, suppA, suppB);

        if (simplex.isAffinelyDependent()) {
            return false;
        }

        if (!simplex.computeClosestPoint(v)) {
            return false;
        }

        // Store and update the square distance
        prevDistSquare = distSquare;
        distSquare = v.lengthSquare();

        if (prevDistSquare - distSquare <= MACHINE_EPSILON * prevDistSquare) {
            return false;
        }

    } while(!simplex.isFull() && distSquare > MACHINE_EPSILON * simplex.getMaxLengthSquareOfAPoint());

    // Give the simplex computed with GJK algorithm to the EPA algorithm which will compute the correct
    // penetration depth and contact points between the two enlarged objects
    return algoEPA.computePenetrationDepthAndContactPoints(simplex, shape1, transform1, shape2, transform2, v, contactInfo);
}