CollisionChecker.cpp

/VirtualRobot/CollisionDetection/CollisionChecker.cpp

https://gitlab.com/terlemez/simox
C++ | 763 lines | 423 code | 112 blank | 228 comment | 100 complexity | c514591f6a0d98dc13a95d1d328880d9 MD5 | raw file


#include "CollisionChecker.h"
#include "CollisionModel.h"
#include "../SceneObjectSet.h"
#include "../SceneObject.h"
#include "../Robot.h"
#include "../VirtualRobotException.h"

#include <cfloat>

#include <Eigen/Core>
#include <Eigen/Geometry>


#if defined(VR_COLLISION_DETECTION_PQP)
#define COL_CHECKER_IMPL CollisionCheckerPQP
#else
#define COL_CHECKER_IMPL CollisionCheckerDummy
#endif




namespace VirtualRobot
{

    namespace
    {
        boost::mutex mutex;
    };

    CollisionCheckerPtr CollisionChecker::globalCollisionChecker;

    CollisionChecker::Cleanup::~Cleanup()
    {
        boost::lock_guard<boost::mutex> lock(mutex);
        CollisionChecker::globalCollisionChecker.reset();
    }


    CollisionCheckerPtr CollisionChecker::getGlobalCollisionChecker()
    {
        static Cleanup _Cleanup;

        if (true)
        {
            boost::lock_guard<boost::mutex> lock(mutex);

            if (!globalCollisionChecker)
            {
                globalCollisionChecker.reset(new CollisionChecker());
            }
        }

        return globalCollisionChecker;
    }

    //----------------------------------------------------------------------
    // class CollisionChecker constructor
    //----------------------------------------------------------------------
    CollisionChecker::CollisionChecker()
    {
        initialized = true;
        debugOutput = false; // ENABLES OUTPUT OF COLLISION MODEL TRIANGLES
        automaticSizeCheck = true;

        collisionCheckerImplementation.reset(new COL_CHECKER_IMPL());
    }

    //----------------------------------------------------------------------
    // class CollisionChecker destructor
    //----------------------------------------------------------------------
    CollisionChecker::~CollisionChecker()
    {
    }


    float CollisionChecker::calculateDistance(SceneObjectSetPtr model1, SceneObjectSetPtr model2)
    {
        VR_ASSERT(model1 && model2);
        return calculateDistance(model1, model2, tmpV1, tmpV2, NULL, NULL);
    }

    float CollisionChecker::calculateDistance(CollisionModelPtr model1, SceneObjectSetPtr model2)
    {
        VR_ASSERT(model1 && model2);
        return calculateDistance(model1, model2, tmpV1, tmpV2, NULL, NULL);
    }

    float CollisionChecker::calculateDistance(CollisionModelPtr model1, CollisionModelPtr model2)
    {
        VR_ASSERT(model1 && model2);
        return calculateDistance(model1, model2, tmpV1, tmpV2, NULL, NULL);
    }


    float CollisionChecker::calculateDistance(SceneObjectPtr model1, SceneObjectSetPtr model2)
    {
        VR_ASSERT(model1 && model2);
        RobotPtr r = boost::dynamic_pointer_cast<Robot>(model1);

        if (r)
        {
            SceneObjectSetPtr robotModels = getRobotModels(r);
            return calculateDistance(robotModels, model2);
        }
        else
        {
            return calculateDistance(model1->getCollisionModel(), model2, tmpV1, tmpV2, NULL, NULL);
        }
    }

    float CollisionChecker::calculateDistance(SceneObjectPtr model1, SceneObjectPtr model2)
    {
        VR_ASSERT(model1 && model2);
        RobotPtr r = boost::dynamic_pointer_cast<Robot>(model1);
        RobotPtr r2 = boost::dynamic_pointer_cast<Robot>(model2);

        if (r && r2)
        {
            SceneObjectSetPtr robotModels = getRobotModels(r);
            SceneObjectSetPtr robotModels2 = getRobotModels(r2);
            return calculateDistance(robotModels, robotModels2);
        }
        else if (r && !r2)
        {
            SceneObjectSetPtr robotModels = getRobotModels(r);
            return calculateDistance(model2, robotModels);
        }
        else if (!r && r2)
        {
            SceneObjectSetPtr robotModels2 = getRobotModels(r2);
            return calculateDistance(model1, robotModels2);
        }
        else
        {
            return calculateDistance(model1->getCollisionModel(), model2->getCollisionModel(), tmpV1, tmpV2, NULL, NULL);
        }
    }


    float CollisionChecker::calculateDistance(SceneObjectPtr model1, SceneObjectSetPtr model2, Eigen::Vector3f& P1, Eigen::Vector3f& P2, int* trID1, int* trID2)
    {
        VR_ASSERT(model1 && model2);
        RobotPtr r = boost::dynamic_pointer_cast<Robot>(model1);

        if (r)
        {
            SceneObjectSetPtr robotModels = getRobotModels(r);
            return calculateDistance(robotModels, model2, P1, P2, trID1, trID2);
        }
        else
        {
            return calculateDistance(model1->getCollisionModel(), model2, P1, P2, trID1, trID2);
        }
    }

    float CollisionChecker::calculateDistance(SceneObjectPtr model1, SceneObjectPtr model2, Eigen::Vector3f& P1, Eigen::Vector3f& P2, int* trID1, int* trID2)
    {
        VR_ASSERT(model1 && model2);
        RobotPtr r = boost::dynamic_pointer_cast<Robot>(model1);
        RobotPtr r2 = boost::dynamic_pointer_cast<Robot>(model2);

        if (r && r2)
        {
            SceneObjectSetPtr robotModels = getRobotModels(r);
            SceneObjectSetPtr robotModels2 = getRobotModels(r2);
            return calculateDistance(robotModels, robotModels2, P1, P2, trID1, trID2);
        }
        else if (r && !r2)
        {
            SceneObjectSetPtr robotModels = getRobotModels(r);
            return calculateDistance(model2, robotModels, P2, P1, trID2, trID1);
        }
        else if (!r && r2)
        {
            SceneObjectSetPtr robotModels2 = getRobotModels(r2);
            return calculateDistance(model1, robotModels2, P1, P2, trID1, trID2);
        }
        else
        {
            return calculateDistance(model1->getCollisionModel(), model2->getCollisionModel(), P1, P2, trID1, trID2);
        }
    }

    float CollisionChecker::calculateDistance(SceneObjectSetPtr model1, SceneObjectSetPtr model2, Eigen::Vector3f& P1, Eigen::Vector3f& P2, int* trID1, int* trID2)
    {
        VR_ASSERT(model1 && model2);
        VR_ASSERT_MESSAGE(model1->getCollisionChecker() == model2->getCollisionChecker(), "Collision models are linked to different Collision Checker instances");
        VR_ASSERT_MESSAGE(model1->getCollisionChecker() == shared_from_this(), "Collision models are linked to different Collision Checker instances");
        VR_ASSERT(isInitialized());

        std::vector< CollisionModelPtr > colModels1 = model1->getCollisionModels();
        std::vector< CollisionModelPtr > colModels2 = model2->getCollisionModels();

        if (colModels1.size() == 0 || colModels2.size() == 0)
        {
            VR_WARNING << "no internal data..." << endl;
            return -1.0f;
        }

        float fResult = FLT_MAX;
        Eigen::Vector3f v1;
        Eigen::Vector3f v2;
        int trID1_r;
        int trID2_r;
        std::vector< CollisionModelPtr >::iterator it1 = colModels1.begin();

        while (it1 != colModels1.end())
        {
            std::vector< CollisionModelPtr >::iterator it2 = colModels2.begin();

            while (it2 != colModels2.end())
            {
                float fRes = calculateDistance(*it1, *it2, v1, v2, &trID1_r, &trID2_r);

                if (fRes <= fResult)
                {
                    fResult = fRes;
                    P1 = v1;
                    P2 = v2;

                    if (trID1)
                    {
                        *trID1 = trID1_r;
                    }

                    if (trID2)
                    {
                        *trID2 = trID2_r;
                    }
                }

                it2++;
            }

            it1++;
        }

        return fResult;
    }

    float CollisionChecker::calculateDistance(CollisionModelPtr model1, SceneObjectSetPtr model2, Eigen::Vector3f& P1, Eigen::Vector3f& P2, int* trID1, int* trID2)
    {
        VR_ASSERT(model1 && model2);
        VR_ASSERT_MESSAGE(model1->getCollisionChecker() == model2->getCollisionChecker(), "Collision models are linked to different Collision Checker instances");
        VR_ASSERT_MESSAGE(model1->getCollisionChecker() == shared_from_this(), "Collision models are linked to different Collision Checker instances");
        VR_ASSERT(isInitialized());

        std::vector< CollisionModelPtr > colModels = model2->getCollisionModels();

        if (colModels.size() == 0)
        {
            VR_WARNING << "no internal data..." << endl;
            return -1.0f;
        }

        float fResult = FLT_MAX;
        Eigen::Vector3f v1;
        Eigen::Vector3f v2;
        int trID1_r;
        int trID2_r;
        std::vector< CollisionModelPtr >::iterator it = colModels.begin();

        while (it != colModels.end())
        {
            float fRes = calculateDistance(model1, *it, v1, v2, &trID1_r, &trID2_r);

            if (fRes <= fResult)
            {
                fResult = fRes;
                P1 = v1;
                P2 = v2;

                if (trID1)
                {
                    *trID1 = trID1_r;
                }

                if (trID2)
                {
                    *trID2 = trID2_r;
                }
            }

            it++;
        }

        return fResult;
    }

    float CollisionChecker::calculateDistance(CollisionModelPtr model1, CollisionModelPtr model2, Eigen::Vector3f& P1, Eigen::Vector3f& P2, int* trID1, int* trID2)
    {
        VR_ASSERT(model1 && model2);
        VR_ASSERT_MESSAGE(model1->getCollisionChecker() == model2->getCollisionChecker(), "Collision models are linked to different Collision Checker instances");
        VR_ASSERT_MESSAGE(model1->getCollisionChecker() == shared_from_this(), "Collision models are linked to different Collision Checker instances");
        VR_ASSERT(isInitialized());

        return collisionCheckerImplementation->calculateDistance(model1, model2, P1, P2, trID1, trID2);
    }



    bool CollisionChecker::checkCollision(std::vector<CollisionModelPtr>& model1, CollisionModelPtr model2)
    {
        VR_ASSERT(model2);
        VR_ASSERT(isInitialized());

        if (model1.size() == 0)
        {
            VR_WARNING << "no internal data..." << endl;
            return false;
        }

        std::vector< CollisionModelPtr >::iterator it1 = model1.begin();

        while (it1 != model1.end())
        {
            if (checkCollision(*it1, model2))
            {
                return true;
            }

            it1++;
        }

        return false;
    }

    bool CollisionChecker::checkCollision(SceneObjectSetPtr model1, SceneObjectSetPtr model2)
    {
        VR_ASSERT(model1 && model2);
        VR_ASSERT_MESSAGE(model1->getCollisionChecker() == model2->getCollisionChecker(), "Collision models are linked to different Collision Checker instances");
        VR_ASSERT_MESSAGE(model1->getCollisionChecker() == shared_from_this(), "Collision models are linked to different Collision Checker instances");
        VR_ASSERT(isInitialized());

        std::vector< CollisionModelPtr > vColModels1 = model1->getCollisionModels();
        std::vector< CollisionModelPtr > vColModels2 = model2->getCollisionModels();

        if (vColModels1.size() == 0 || vColModels2.size() == 0)
        {
            VR_WARNING << "no internal data..." << endl;
            return false;
        }

        std::vector< CollisionModelPtr >::iterator it1 = vColModels1.begin();

        while (it1 != vColModels1.end())
        {
            std::vector< CollisionModelPtr >::iterator it2 = vColModels2.begin();

            while (it2 != vColModels2.end())
            {
                if (checkCollision(*it1, *it2))
                {
                    return true;
                }

                it2++;
            }

            it1++;
        }

        return false;
    }

    bool CollisionChecker::checkCollision(SceneObjectPtr model1, SceneObjectSetPtr model2)
    {
        VR_ASSERT(model1 && model2);
        RobotPtr r = boost::dynamic_pointer_cast<Robot>(model1);

        if (r)
        {
            SceneObjectSetPtr robotModels = getRobotModels(r);
            return checkCollision(robotModels, model2);
        }
        else
        {
            return checkCollision(model1->getCollisionModel(), model2);
        }
    }

    SceneObjectSetPtr CollisionChecker::getRobotModels(RobotPtr r)
    {
        SceneObjectSetPtr result(new SceneObjectSet(r->getName(), shared_from_this()));
        std::vector<RobotNodePtr> cm = r->getRobotNodes();

        for (size_t i = 0; i < cm.size(); i++)
        {
            if (cm[i]->getCollisionModel())
            {
                result->addSceneObject(cm[i]);
            }
        }

        return result;
    }


    bool CollisionChecker::checkCollision(CollisionModelPtr model1, SceneObjectSetPtr model2)
    {
        VR_ASSERT(model1 && model2);
        VR_ASSERT_MESSAGE(model1->getCollisionChecker() == model2->getCollisionChecker(), "Collision models are linked to different Collision Checker instances");
        VR_ASSERT_MESSAGE(model1->getCollisionChecker() == shared_from_this(), "Collision models are linked to different Collision Checker instances");
        VR_ASSERT(isInitialized());

        std::vector< CollisionModelPtr > vColModels = model2->getCollisionModels();

        if (vColModels.size() == 0)
        {
            VR_WARNING << "no internal data..." << endl;
            return false;
        }

        std::vector< CollisionModelPtr >::iterator it = vColModels.begin();

        while (it != vColModels.end())
        {
            if (checkCollision(model1, *it))
            {
                return true;
            }

            it++;
        }

        return false;
    }

    bool CollisionChecker::checkCollision(SceneObjectPtr model1, SceneObjectPtr model2)
    {
        VR_ASSERT(model1 && model2);
        RobotPtr r = boost::dynamic_pointer_cast<Robot>(model1);
        RobotPtr r2 = boost::dynamic_pointer_cast<Robot>(model2);

        if (r && r2)
        {
            SceneObjectSetPtr robotModels = getRobotModels(r);
            SceneObjectSetPtr robotModels2 = getRobotModels(r2);
            return checkCollision(robotModels, robotModels2);
        }
        else if (r && !r2)
        {
            SceneObjectSetPtr robotModels = getRobotModels(r);
            return checkCollision(model2, robotModels);
        }
        else if (!r && r2)
        {
            SceneObjectSetPtr robotModels2 = getRobotModels(r2);
            return checkCollision(model1, robotModels2);
        }
        else
        {
            return checkCollision(model1->getCollisionModel(), model2->getCollisionModel());
        }
    }

    bool CollisionChecker::checkCollision(CollisionModelPtr model1, CollisionModelPtr model2)
    {
        VR_ASSERT(model1 && model2);
        VR_ASSERT_MESSAGE(model1->getCollisionChecker() == model2->getCollisionChecker(), "Collision models are linked to different Collision Checker instances");
        VR_ASSERT_MESSAGE(model1->getCollisionChecker() == shared_from_this(), "Collision models are linked to different Collision Checker instances");
        VR_ASSERT(isInitialized());

        return collisionCheckerImplementation->checkCollision(model1, model2);//, storeContact);
    }

    /*
    bool CollisionChecker::getAllCollisonTriangles (SceneObjectSetPtr model1, SceneObjectSetPtr model2, std::vector<int> &storePairs)
    {
        if (!model1 || !model2)
        {
            printf ("CollisionChecker:GetAllCollisongTriangles - NULL data...\n");
            return false;
        }
        if (model1->getCollisionChecker() != model2->getCollisionChecker() || model1->getCollisionChecker()!=shared_from_this())
        {
            VR_WARNING << "Could not go on, collision models are linked to different Collision Checker instances." << endl;
            return false;
        }
        if (!isInitialized())
        {
            VR_WARNING << "not initialized." << endl;
            return false;
        }
        return collisionCheckerImplementation->getAllCollisonTriangles(model1,model2,storePairs);
    }*/


    void CollisionChecker::setAutomaticSizeCheck(bool checkSizeOnColModelCreation)
    {
        automaticSizeCheck = checkSizeOnColModelCreation;
        collisionCheckerImplementation->setAutomaticSizeCheck(automaticSizeCheck);
    }

    /*
    bool CollisionChecker::checkCollision( SbXfBox3f& box1, SbXfBox3f& box2 )
    {
        ////return box1.intersect(box2);

        //box1.setBounds(-100, -100, -100, 100, 100, 100);
        //box2.setBounds(-100, -100, -100, 100, 100, 100);

        //SbMatrix tr;
        //tr.setTranslate(SbVec3f(0,0,0));
        //box1.setTransform(tr);

        //tr.setTranslate(SbVec3f(0, 0, 250));
        //box2.setTransform(tr);

        // OBB Intersection test from Neoengine

        const float fParallellCutoff = 0.99999f;
        bool bParallellAxes = false;

        SbRotation kRotA;
        SbRotation kRotB;
        SbVec3f kTransA;
        SbVec3f kTransB;
        SbMatrix kRotMatA;
        SbMatrix kRotMatB;
        SbVec3f tmpV;
        SbRotation tmpR;

        box1.getTransform().getTransform(kTransA, kRotA, tmpV, tmpR);
        box2.getTransform().getTransform(kTransB, kRotB, tmpV, tmpR);
        kRotMatA.setRotate(kRotA);
        kRotMatB.setRotate(kRotB);

        SbVec3f akAxesA[3] = { SbVec3f(kRotMatA[0]), SbVec3f(kRotMatA[1]), SbVec3f(kRotMatA[2]) };
        SbVec3f akAxesB[3] = { SbVec3f(kRotMatB[0]), SbVec3f(kRotMatB[1]), SbVec3f(kRotMatB[2]) };

        float afDimA[3];
        float afDimB[3];

        float dx, dy, dz;
        box1.getSize(dx, dy, dz);
        afDimA[0] = dx/2;
        afDimA[1] = dy/2;
        afDimA[2] = dz/2;

        box2.getSize(dx, dy, dz);
        afDimB[0] = dx/2;
        afDimB[1] = dy/2;
        afDimB[2] = dz/2;

        //Difference of box positions
        SbVec3f kDiff = kTransB - kTransA;

        ////////Early out test
        //float radiusA2 = 0, radiusB2 = 0;

        //SbBox3f aabb1(box1);
        //SbBox3f aabb2(box2);
        //radiusA2 = (aabb1.getMax()-aabb1.getCenter()).length();
        //radiusB2 = (aabb2.getMax()-aabb2.getCenter()).length();
        //if( ( radiusA2 + radiusB2 ) < kDiff.length() )
        //  return false;

        float afAxisDot[3][3];    // afAxisDot[i][j]    = akAxesA[i]Ptr  akAxesB[j];
        float afAbsAxisDot[3][3]; // afAbsAxisDot[i][j] = |afAxisDot[i][j]|

        float afAxesADotDiff[3];  // afAxesADotDiff[i]  = akAxesA[i]Ptr  kDiff

        //Test all 15 possible separating axes

        //Axis Ax
        //First calculate AxPtr  Bi axis dot products (length of each B axis along Ax)
        afAxisDot[0][0] = akAxesA[0].dot(akAxesB[0]);
        afAxisDot[0][1] = akAxesA[0].dot(akAxesB[1]);
        afAxisDot[0][2] = akAxesA[0].dot(akAxesB[2]);

        //Get absolute value of dot products
        afAbsAxisDot[0][0] = fabsf( afAxisDot[0][0] ); if( afAbsAxisDot[0][0] > fParallellCutoff ) bParallellAxes = true;
        afAbsAxisDot[0][1] = fabsf( afAxisDot[0][1] ); if( afAbsAxisDot[0][1] > fParallellCutoff ) bParallellAxes = true;
        afAbsAxisDot[0][2] = fabsf( afAxisDot[0][2] ); if( afAbsAxisDot[0][2] > fParallellCutoff ) bParallellAxes = true;

        //calculate AxPtr  D dot product (length of center difference along Ax axis)
        afAxesADotDiff[0]  = akAxesA[0].dot(kDiff);

        //  int     iDeepAxis = 0;
        //  SbVec3f kNormal;

        //  float fOverlapMax  = -numeric_limits< float >::max();
        float fOverlap;

        //Check if distance between centers along axis is greater than sum of boxes dimensions along axis
        if( ( fOverlap = fabsf( afAxesADotDiff[0] ) - ( ( afDimA[0] ) + ( afDimB[0]Ptr  afAbsAxisDot[0][0] + afDimB[1]Ptr  afAbsAxisDot[0][1] + afDimB[2]Ptr  afAbsAxisDot[0][2] ) ) ) > 0.0f )
            return false;

        //Axis Ay
        afAxisDot[1][0] = akAxesA[1].dot(akAxesB[0]);
        afAxisDot[1][1] = akAxesA[1].dot(akAxesB[1]);
        afAxisDot[1][2] = akAxesA[1].dot(akAxesB[2]);

        afAbsAxisDot[1][0] = fabsf( afAxisDot[1][0] ); if( afAbsAxisDot[1][0] > fParallellCutoff ) bParallellAxes = true;
        afAbsAxisDot[1][1] = fabsf( afAxisDot[1][1] ); if( afAbsAxisDot[1][1] > fParallellCutoff ) bParallellAxes = true;
        afAbsAxisDot[1][2] = fabsf( afAxisDot[1][2] ); if( afAbsAxisDot[1][2] > fParallellCutoff ) bParallellAxes = true;

        afAxesADotDiff[1]  = akAxesA[1].dot(kDiff);

        if( ( fOverlap = fabsf( afAxesADotDiff[1] ) - ( ( afDimA[1] ) + ( afDimB[0]Ptr  afAbsAxisDot[1][0] + afDimB[1]Ptr  afAbsAxisDot[1][1] + afDimB[2]Ptr  afAbsAxisDot[1][2] ) ) ) > 0.0f )
            return false;

        //Axis Az
        afAxisDot[2][0] = akAxesA[2].dot(akAxesB[0]);
        afAxisDot[2][1] = akAxesA[2].dot(akAxesB[1]);
        afAxisDot[2][2] = akAxesA[2].dot(akAxesB[2]);

        afAbsAxisDot[2][0] = fabsf( afAxisDot[2][0] ); if( afAbsAxisDot[2][0] > fParallellCutoff ) bParallellAxes = true;
        afAbsAxisDot[2][1] = fabsf( afAxisDot[2][1] ); if( afAbsAxisDot[2][1] > fParallellCutoff ) bParallellAxes = true;
        afAbsAxisDot[2][2] = fabsf( afAxisDot[2][2] ); if( afAbsAxisDot[2][2] > fParallellCutoff ) bParallellAxes = true;

        afAxesADotDiff[2]  = akAxesA[2].dot(kDiff);

        if( ( fOverlap = fabsf( afAxesADotDiff[2] ) - ( ( afDimA[2] ) + ( afDimB[0]Ptr  afAbsAxisDot[2][0] + afDimB[1]Ptr  afAbsAxisDot[2][1] + afDimB[2]Ptr  afAbsAxisDot[2][2] ) ) ) > 0.0f )
            return false;

        float fProj;
        //  float fScale;

        //Axis Bx
        //We already have all axis*axis dot products, only calculate center difference along Bx axis and compare
        if( ( fOverlap = fabsf( ( fProj = akAxesB[0].dot(kDiff) ) ) - ( ( afDimA[0]Ptr  afAbsAxisDot[0][0] + afDimA[1]Ptr  afAbsAxisDot[1][0] + afDimA[2]Ptr  afAbsAxisDot[2][0] ) + ( afDimB[0] ) ) ) > 0.0f )
            return false;

        //Axis By
        if( ( fOverlap = fabsf( ( fProj = akAxesB[1].dot(kDiff) ) ) - ( ( afDimA[0]Ptr  afAbsAxisDot[0][1] + afDimA[1]Ptr  afAbsAxisDot[1][1] + afDimA[2]Ptr  afAbsAxisDot[2][1] ) + ( afDimB[1] ) ) ) > 0.0f )
            return false;

        //Axis Bz
        if( ( fOverlap = fabsf( ( fProj = akAxesB[2].dot(kDiff) ) ) - ( ( afDimA[0]Ptr  afAbsAxisDot[0][2] + afDimA[1]Ptr  afAbsAxisDot[1][2] + afDimA[2]Ptr  afAbsAxisDot[2][2] ) + ( afDimB[2] ) ) ) > 0.0f )
            return false;

        //if not contact set, do extra tests to avoid reporting false collisions in parallell axis threshold zone
        if( !bParallellAxes  )
        {

            //Axis Ax X Bx
            if( ( fOverlap = fabsf( ( fProj = afAxisDot[1][0]Ptr  afAxesADotDiff[2] - afAxisDot[2][0]Ptr  afAxesADotDiff[1] ) ) -
                ( ( afDimA[1]Ptr  afAbsAxisDot[2][0] + afDimA[2]Ptr  afAbsAxisDot[1][0] ) + ( afDimB[1]Ptr  afAbsAxisDot[0][2] + afDimB[2]Ptr  afAbsAxisDot[0][1] ) ) ) > 0.0f )
                return false;

            //Axis Ax X By
            if( ( fOverlap = fabsf( ( fProj = afAxisDot[1][1]Ptr  afAxesADotDiff[2] - afAxisDot[2][1]Ptr  afAxesADotDiff[1] ) ) -
                ( ( afDimA[1]Ptr  afAbsAxisDot[2][1] + afDimA[2]Ptr  afAbsAxisDot[1][1] ) + ( afDimB[0]Ptr  afAbsAxisDot[0][2] + afDimB[2]Ptr  afAbsAxisDot[0][0] ) ) ) > 0.0f )
                return false;

            //Axis Ax X Bz
            if( ( fOverlap = fabsf( ( fProj = afAxisDot[1][2]Ptr  afAxesADotDiff[2] - afAxisDot[2][2]Ptr  afAxesADotDiff[1] ) ) -
                ( ( afDimA[1]Ptr  afAbsAxisDot[2][2] + afDimA[2]Ptr  afAbsAxisDot[1][2] ) + ( afDimB[0]Ptr  afAbsAxisDot[0][1] + afDimB[1]Ptr  afAbsAxisDot[0][0] ) ) ) > 0.0f )
                return false;

            //Axis Ay X Bx
            if( ( fOverlap = fabsf( ( fProj = afAxisDot[2][0]Ptr  afAxesADotDiff[0] - afAxisDot[0][0]Ptr  afAxesADotDiff[2] ) ) -
                ( ( afDimA[0]Ptr  afAbsAxisDot[2][0] + afDimA[2]Ptr  afAbsAxisDot[0][0] ) + ( afDimB[1]Ptr  afAbsAxisDot[1][2] + afDimB[2]Ptr  afAbsAxisDot[1][1] ) ) ) > 0.0f )
                return false;

            //Axis Ay X By
            if( ( fOverlap = fabsf( ( fProj = afAxisDot[2][1]Ptr  afAxesADotDiff[0] - afAxisDot[0][1]Ptr  afAxesADotDiff[2] ) ) -
                ( ( afDimA[0]Ptr  afAbsAxisDot[2][1] + afDimA[2]Ptr  afAbsAxisDot[0][1] ) + ( afDimB[0]Ptr  afAbsAxisDot[1][2] + afDimB[2]Ptr  afAbsAxisDot[1][0] ) ) ) > 0.0f )
                return false;

            //Axis Ay X Bz
            if( ( fOverlap = fabsf( ( fProj = afAxisDot[2][2]Ptr  afAxesADotDiff[0] - afAxisDot[0][2]Ptr  afAxesADotDiff[2] ) ) -
                ( ( afDimA[0]Ptr  afAbsAxisDot[2][2] + afDimA[2]Ptr  afAbsAxisDot[0][2] ) + ( afDimB[0]Ptr  afAbsAxisDot[1][1] + afDimB[1]Ptr  afAbsAxisDot[1][0] ) ) ) > 0.0f )
                return false;

            //Axis Az X Bx
            if( ( fOverlap = fabsf( ( fProj = afAxisDot[0][0]Ptr  afAxesADotDiff[1] - afAxisDot[1][0]Ptr  afAxesADotDiff[0] ) ) -
                ( ( afDimA[0]Ptr  afAbsAxisDot[1][0] + afDimA[1]Ptr  afAbsAxisDot[0][0] ) + ( afDimB[1]Ptr  afAbsAxisDot[2][2] + afDimB[2]Ptr  afAbsAxisDot[2][1] ) ) ) > 0.0f )
                return false;

            //Axis Az X By
            if( ( fOverlap = fabsf( ( fProj = afAxisDot[0][1]Ptr  afAxesADotDiff[1] - afAxisDot[1][1]Ptr  afAxesADotDiff[0] ) ) -
                ( ( afDimA[0]Ptr  afAbsAxisDot[1][1] + afDimA[1]Ptr  afAbsAxisDot[0][1] ) + ( afDimB[0]Ptr  afAbsAxisDot[2][2] + afDimB[2]Ptr  afAbsAxisDot[2][0] ) ) ) > 0.0f )
                return false;

            //Axis Az X Bz
            if( ( fOverlap = fabsf( ( fProj = afAxisDot[0][2]Ptr  afAxesADotDiff[1] - afAxisDot[1][2]Ptr  afAxesADotDiff[0] ) ) -
                ( ( afDimA[0]Ptr  afAbsAxisDot[1][2] + afDimA[1]Ptr  afAbsAxisDot[0][2] ) + ( afDimB[0]Ptr  afAbsAxisDot[2][1] + afDimB[1]Ptr  afAbsAxisDot[2][0] ) ) ) > 0.0f )
                return false;
        } // if( bParallellAxes )

        return true;
    }
    */

    bool CollisionChecker::IsSupported_CollisionDetection()
    {
        return COL_CHECKER_IMPL::IsSupported_CollisionDetection();
    }

    bool CollisionChecker::IsSupported_ContinuousCollisionDetection()
    {
        return COL_CHECKER_IMPL::IsSupported_ContinuousCollisionDetection();
    }

    bool CollisionChecker::IsSupported_DistanceCalculations()
    {
        return COL_CHECKER_IMPL::IsSupported_DistanceCalculations();
    }

    bool CollisionChecker::IsSupported_Multithreading_Threadsafe()
    {
        return COL_CHECKER_IMPL::IsSupported_Multithreading_Threadsafe();
    }

    bool CollisionChecker::IsSupported_Multithreading_MultipleColCheckers()
    {
        return COL_CHECKER_IMPL::IsSupported_Multithreading_MultipleColCheckers();
    }

    void CollisionChecker::getContacts(const MathTools::Plane& p, CollisionModelPtr colModel, std::vector< MathTools::ContactPoint >& storeContatcs, float maxDist /*= 1.0f*/)
    {
        THROW_VR_EXCEPTION_IF(!colModel, "NULl data");

        // first check if plane hits bounding box
        BoundingBox bbox = colModel->getBoundingBox(false);
        // enlarge bbox by maxDist
        bbox.min -= Eigen::Vector3f(maxDist, maxDist, maxDist);
        bbox.max += Eigen::Vector3f(maxDist, maxDist, maxDist);

        std::vector <Eigen::Vector3f> ptsBB = bbox.getPoints();

        for (size_t i = 0; i < ptsBB.size(); i++)
        {
            ptsBB[i] = MathTools::transformPosition(ptsBB[i], colModel->getGlobalPose());
        }

        BoundingBox bboxGlobal(ptsBB);

        if (!bboxGlobal.planeGoesThrough(p))
        {
            // plane is not going through bounding box
            return;
        }

        // bbox was hit, test all points...
        std::vector< Eigen::Vector3f > pts = colModel->getModelVeticesGlobal();

        for (std::vector< Eigen::Vector3f >::iterator i = pts.begin(); i != pts.end(); i++)
        {
            if (MathTools::getDistancePointPlane(*i, p) <= maxDist)
            {
                MathTools::ContactPoint contact;
                contact.n = p.n;
                contact.p = *i;
                storeContatcs.push_back(contact);
            }
        }
    }

    /*
    bool CollisionChecker::checkContinuousCollision( CollisionModelPtr model1, SbMatrix &mGoalPose1, CollisionModelPtr model2, SbMatrix &mGoalPose2, float &fStoreTOC )
    {
        return collisionCheckerImplementation->checkContinuousCollision(model1,mGoalPose1,model2,mGoalPose2, fStoreTOC);
    }
    */

} // namespace VirtualRobot