colmodel.cpp

Go to the documentation of this file.

//#define WANT_TRACE

/* MAINTAINER: Petko CURRENT EDITOR: Bernard */
#include "colmodel.h"
#include "globals.h"
#include "log.h"

const double EPSILON = 0.00001;




CollisionModel::CollisionModel()
{
        mReseted = true;
        mBox.zeroize();
        mTransBox.zeroize();
        mTranslation.set( 0, 0, 0 );
        mRotation.set( ZERO_ROTATION_VECTOR, 0 );
        mAvoidVector.set( 0, 0, 0);
}

#define SEP_TEST(box1, box2, a1, a2, b1, b2) \
        if ( absD((d=box1.trySepparatingAxis(box2, a1, a2, b1, b2, normal))) < absD(min)) \
        { \
                if (absD(min = d) < EPSILON) return false; \
                result = normal; \
        }

#define SEPSELF_TEST(box1, box2, a1, a2, b1, b2) \
        if ( absD((d=box1.trySepparatingAxisSelf(box2, a1, a2, b1, b2, normal))) < absD(min)) \
        { \
                if (absD(min = d) < EPSILON) return false; \
                result = normal; \
        }

#define SEPSELF_MINUSTEST(box1, box2, a1, a2, b1, b2) \
        if ( absD((d=box1.trySepparatingAxisSelf(box2, a1, a2, b1, b2, normal))) < absD(min)) \
        { \
                if (absD(min = -d) < EPSILON) return false; \
                result = normal; \
        }

bool CollisionModel::computeMinimalAvoidVector(const CollisionModel & model, GLVector3d & result)
{
        // quick check - not needed to get correct results,
        // but speed up the testing
        if (!mTransBox.quickCheckCollision(model.mTransBox)) 
        {
                return false;
        }
        
        const CDBox &box1 = mTransBox;
        const CDBox &box2 = model.mTransBox;
        
        GLVector3d normal;
        double d, min = 10000;
        
        // Testing planes defined by both boxes
        for (int i = 0; i < 2; i++) 
        {
                for (int j = 0; j < 2; j++)
                        SEP_TEST(box1, box2, i, i+1, j, j+1);
                
                SEP_TEST(box1, box2, i, i+1, 0,   4);
                SEP_TEST(box1, box2, 0,   4, i, i+1);
        }
        SEP_TEST(box1, box2, 0, 4, 0, 4);
        
        // Testing planes defined by our box
        SEPSELF_TEST(box1, box2, 0, 1, 1, 2);
        SEPSELF_TEST(box1, box2, 1, 2, 1, 5);
        SEPSELF_TEST(box1, box2, 0, 1, 0, 4);
        
        // Testing planes defined by second box [note that the result mus be mutiplyed by -1]
        SEPSELF_MINUSTEST(box1, box2, 0, 1, 1, 2);
        SEPSELF_MINUSTEST(box2, box1, 0, 1, 1, 2);
        SEPSELF_MINUSTEST(box2, box1, 1, 2, 1, 5);
        SEPSELF_MINUSTEST(box2, box1, 0, 1, 0, 4);
        
        // Create avoid vector from normal of minimal separating plane
        result *= min;
                
        return true;
}

        double projectAndCompareMax(const CDBox & box1, const CDBox & box2, const GLVector3d &normal, double scale)
        {               
                // Get min / max for first box
                double min1, max1;
                box1.project(normal, min1, max1);
                
                // Get min / max for second box
                double min2, max2;
                box2.project(normal, min2, max2);
        
                // Decide the colision
                if (min2 >= max1 || min1 >= max2) 
                {
                        return 0; // No collision
                }
                
                if (scale == 0) return 100000;
                if ((max2 - min1)/scale > 0) return (max2 - min1)/scale;
                return (min2 - max1)/scale;
        }

        double trySepparatingAxisMax(const CDBox &box1, const CDBox &box2, int a1, int a2,
                                             int b1, int b2, GLVector3d &normal, const GLVector3d &direction)
        {
                // compute normal vector of seprarating plane
                normal = box1.mVertex[a1]-box1.mVertex[a2];
                normal.cross(box2.mVertex[b1]-box2.mVertex[b2]);
                
                if (normal.length() == 0) 
                {
                        return 100000000;               
                }
                normal.normalize();
        
                double scale = normal.dot(direction);
                        
                return projectAndCompareMax(box1, box2, normal, scale);
        }

        double trySepparatingAxisSelfMax(const CDBox &box1, const CDBox &box2, int a1, int a2,
                                              int b1, int b2, GLVector3d &normal, const GLVector3d &direction)
        {
                // compute normal vector of seprarating plane
                normal = box1.mVertex[a1]-box1.mVertex[a2];
                normal.cross(box1.mVertex[b1]-box1.mVertex[b2]);
                
                if (normal.length() == 0) 
                {
                        return 100000000;               
                }
                normal.normalize();
        
                double scale = normal.dot(direction);
                
                return projectAndCompareMax(box1, box2, normal, scale);
        }


#define SEP_TESTMAX(box1, box2, a1, a2, b1, b2) \
        if ( (d = trySepparatingAxisMax(box1, box2, a1, a2, b1, b2, normal, direction)) <  min) \
        { \
                if ((min = d) < EPSILON) return 0; \
                if (min < 0) \
                 LOGE " POZOR!!!" << endl; \
        }

#define SEPSELF_TESTMAX(box1, box2, a1, a2, b1, b2) \
        if ( (d = trySepparatingAxisSelfMax(box1, box2, a1, a2, b1, b2, normal, direction)) <  min) \
        { \
                if ((min = d) < EPSILON) return 0; \
                if (min < 0) \
                 LOGE " POZOR!!!" << endl; \
        }

#define SEPSELF_MINUSTESTMAX(box1, box2, a1, a2, b1, b2) \
        if ( (d = trySepparatingAxisSelfMax(box1, box2, a1, a2, b1, b2, normal, -direction)) <  min) \
        { \
                if ((min = d) < EPSILON) return 0; \
                if (min < 0) \
                 LOGE " POZOR!!!" << endl; \
        }

double CollisionModel::computeMinimalAvoidDistance( const CollisionModel & model, GLVector3d & direction)
{
        // quick check - not needed to get correct results,
        // but speed up the testing
        if (!mTransBox.quickCheckCollision(model.mTransBox)) 
        {
                return 0;
        }
        
        const CDBox &box1 = mTransBox;
        const CDBox &box2 = model.mTransBox;
        
        GLVector3d normal;
        double d, min = 10000;
        
        // Testing planes defined by both boxes
        for (int i = 0; i < 2; i++) 
        {
                for (int j = 0; j < 2; j++)             
                        SEP_TESTMAX(box1, box2, i, i+1, j, j+1);
                
                SEP_TESTMAX(box1, box2, i, i+1, 0,   4);
                SEP_TESTMAX(box1, box2, 0,   4, i, i+1);
        }
        SEP_TESTMAX(box1, box2, 0, 4, 0, 4);
        
        // Testing planes defined by our box
        SEPSELF_TESTMAX(box1, box2, 0, 1, 1, 2);
        SEPSELF_TESTMAX(box1, box2, 1, 2, 1, 5);
        SEPSELF_TESTMAX(box1, box2, 0, 1, 0, 4);
        
        // Testing planes defined by second box [note that the result mus be mutiplyed by -1]
        SEPSELF_MINUSTESTMAX(box1, box2, 0, 1, 1, 2);
        SEPSELF_MINUSTESTMAX(box2, box1, 0, 1, 1, 2);
        SEPSELF_MINUSTESTMAX(box2, box1, 1, 2, 1, 5);
        SEPSELF_MINUSTESTMAX(box2, box1, 0, 1, 0, 4);
        
        return min;

}

---