OPC_OBBCollider.cpp

Go to the documentation of this file.

/*
 *  OPCODE - Optimized Collision Detection
 *  Copyright (C) 2001 Pierre Terdiman
 *  Homepage: http://www.codercorner.com/Opcode.htm
 */





// Precompiled Header
#include "Stdafx.h"


using namespace Opcode;

#include "OPC_BoxBoxOverlap.h"
#include "OPC_TriBoxOverlap.h"

#define SET_CONTACT(prim_index, flag)                                           \
    /* Set contact status */                                                    \
    mFlags |= flag;                                                             \
    mTouchedPrimitives->Add(prim_index);

#define OBB_PRIM(prim_index, flag)                                              \
    /* Request vertices from the app */                                         \
    VertexPointers VP;  mIMesh->GetTriangle(VP, prim_index);                    \
    /* Transform them in a common space */                                      \
    TransformPoint(mLeafVerts[0], *VP.Vertex[0], mRModelToBox, mTModelToBox);   \
    TransformPoint(mLeafVerts[1], *VP.Vertex[1], mRModelToBox, mTModelToBox);   \
    TransformPoint(mLeafVerts[2], *VP.Vertex[2], mRModelToBox, mTModelToBox);   \
    /* Perform triangle-box overlap test */                                     \
    if(TriBoxOverlap())                                                         \
    {                                                                           \
        SET_CONTACT(prim_index, flag)                                           \
    }


OBBCollider::OBBCollider() : mFullBoxBoxTest(true)
{
}


OBBCollider::~OBBCollider()
{
}


const char* OBBCollider::ValidateSettings()
{
    if(TemporalCoherenceEnabled() && !FirstContactEnabled())    return "Temporal coherence only works with ""First contact"" mode!";

    return VolumeCollider::ValidateSettings();
}


bool OBBCollider::Collide(OBBCache& cache, const OBB& box, const Model& model, const Matrix4x4* worldb, const Matrix4x4* worldm)
{
    // Checkings
    if(!Setup(&model))  return false;

    // Init collision query
    if(InitQuery(cache, box, worldb, worldm))   return true;

    if(!model.HasLeafNodes())
    {
        if(model.IsQuantized())
        {
            const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();

            // Setup dequantization coeffs
            mCenterCoeff    = Tree->mCenterCoeff;
            mExtentsCoeff   = Tree->mExtentsCoeff;

            // Perform collision query
            if(SkipPrimitiveTests())    _CollideNoPrimitiveTest(Tree->GetNodes());
            else                        _Collide(Tree->GetNodes());
        }
        else
        {
            const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();

            // Perform collision query
            if(SkipPrimitiveTests())    _CollideNoPrimitiveTest(Tree->GetNodes());
            else                        _Collide(Tree->GetNodes());
        }
    }
    else
    {
        if(model.IsQuantized())
        {
            const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();

            // Setup dequantization coeffs
            mCenterCoeff    = Tree->mCenterCoeff;
            mExtentsCoeff   = Tree->mExtentsCoeff;

            // Perform collision query
            if(SkipPrimitiveTests())    _CollideNoPrimitiveTest(Tree->GetNodes());
            else                        _Collide(Tree->GetNodes());
        }
        else
        {
            const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();

            // Perform collision query
            if(SkipPrimitiveTests())    _CollideNoPrimitiveTest(Tree->GetNodes());
            else                        _Collide(Tree->GetNodes());
        }
    }

    return true;
}


bool OBBCollider::InitQuery(OBBCache& cache, const OBB& box, const Matrix4x4* worldb, const Matrix4x4* worldm)
{
    // 1) Call the base method
    VolumeCollider::InitQuery();

    // 2) Compute obb in world space
    mBoxExtents = box.mExtents;

    Matrix4x4 WorldB;

    if(worldb)
    {
        WorldB = Matrix4x4( box.mRot * Matrix3x3(*worldb) );
        WorldB.SetTrans(box.mCenter * *worldb);
    }
    else
    {
        WorldB = box.mRot;
        WorldB.SetTrans(box.mCenter);
    }

    // Setup matrices
    Matrix4x4 InvWorldB;
    InvertPRMatrix(InvWorldB, WorldB);

    if(worldm)
    {
        Matrix4x4 InvWorldM;
        InvertPRMatrix(InvWorldM, *worldm);

        Matrix4x4 WorldBtoM = WorldB * InvWorldM;
        Matrix4x4 WorldMtoB = *worldm * InvWorldB;

        mRModelToBox = WorldMtoB;       WorldMtoB.GetTrans(mTModelToBox);
        mRBoxToModel = WorldBtoM;       WorldBtoM.GetTrans(mTBoxToModel);
    }
    else
    {
        mRModelToBox = InvWorldB;   InvWorldB.GetTrans(mTModelToBox);
        mRBoxToModel = WorldB;      WorldB.GetTrans(mTBoxToModel);
    }

    // 3) Setup destination pointer
    mTouchedPrimitives = &cache.TouchedPrimitives;

    // 4) Special case: 1-triangle meshes [Opcode 1.3]
    if(mCurrentModel && mCurrentModel->HasSingleNode())
    {
        if(!SkipPrimitiveTests())
        {
            // We simply perform the BV-Prim overlap test each time. We assume single triangle has index 0.
            mTouchedPrimitives->Reset();

            // Perform overlap test between the unique triangle and the box (and set contact status if needed)
            OBB_PRIM(udword(0), OPC_CONTACT)

            // Return immediately regardless of status
            return TRUE;
        }
    }

    // 5) Check temporal coherence:
    if(TemporalCoherenceEnabled())
    {
        // Here we use temporal coherence
        // => check results from previous frame before performing the collision query
        if(FirstContactEnabled())
        {
            // We're only interested in the first contact found => test the unique previously touched face
            if(mTouchedPrimitives->GetNbEntries())
            {
                // Get index of previously touched face = the first entry in the array
                udword PreviouslyTouchedFace = mTouchedPrimitives->GetEntry(0);

                // Then reset the array:
                // - if the overlap test below is successful, the index we'll get added back anyway
                // - if it isn't, then the array should be reset anyway for the normal query
                mTouchedPrimitives->Reset();

                // Perform overlap test between the cached triangle and the box (and set contact status if needed)
                OBB_PRIM(PreviouslyTouchedFace, OPC_TEMPORAL_CONTACT)

                // Return immediately if possible
                if(GetContactStatus())  return TRUE;
            }
            // else no face has been touched during previous query
            // => we'll have to perform a normal query
        }
        else
        {
            // ### rewrite this
            OBB TestBox(mTBoxToModel, mBoxExtents, mRBoxToModel);

            // We're interested in all contacts =>test the new real box N(ew) against the previous fat box P(revious):
            if(IsCacheValid(cache) && TestBox.IsInside(cache.FatBox))
            {
                // - if N is included in P, return previous list
                // => we simply leave the list (mTouchedFaces) unchanged

                // Set contact status if needed
                if(mTouchedPrimitives->GetNbEntries())  mFlags |= OPC_TEMPORAL_CONTACT;

                // In any case we don't need to do a query
                return TRUE;
            }
            else
            {
                // - else do the query using a fat N

                // Reset cache since we'll about to perform a real query
                mTouchedPrimitives->Reset();

                // Make a fat box so that coherence will work for subsequent frames
                TestBox.mExtents *= cache.FatCoeff;
                mBoxExtents *= cache.FatCoeff;

                // Update cache with query data (signature for cached faces)
                cache.FatBox = TestBox;
            }
        }
    }
    else
    {
        // Here we don't use temporal coherence => do a normal query
        mTouchedPrimitives->Reset();
    }

    // Now we can precompute box-box data

    // Precompute absolute box-to-model rotation matrix
    for(udword i=0;i<3;i++)
    {
        for(udword j=0;j<3;j++)
        {
            // Epsilon value prevents floating-point inaccuracies (strategy borrowed from RAPID)
            mAR.m[i][j] = 1e-6f + fabsf(mRBoxToModel.m[i][j]);
        }
    }

    // Precompute bounds for box-in-box test
    mB0 = mBoxExtents - mTModelToBox;
    mB1 = - mBoxExtents - mTModelToBox;

    // Precompute box-box data - Courtesy of Erwin de Vries
    mBBx1 = mBoxExtents.x*mAR.m[0][0] + mBoxExtents.y*mAR.m[1][0] + mBoxExtents.z*mAR.m[2][0];
    mBBy1 = mBoxExtents.x*mAR.m[0][1] + mBoxExtents.y*mAR.m[1][1] + mBoxExtents.z*mAR.m[2][1];
    mBBz1 = mBoxExtents.x*mAR.m[0][2] + mBoxExtents.y*mAR.m[1][2] + mBoxExtents.z*mAR.m[2][2];

    mBB_1 = mBoxExtents.y*mAR.m[2][0] + mBoxExtents.z*mAR.m[1][0];
    mBB_2 = mBoxExtents.x*mAR.m[2][0] + mBoxExtents.z*mAR.m[0][0];
    mBB_3 = mBoxExtents.x*mAR.m[1][0] + mBoxExtents.y*mAR.m[0][0];
    mBB_4 = mBoxExtents.y*mAR.m[2][1] + mBoxExtents.z*mAR.m[1][1];
    mBB_5 = mBoxExtents.x*mAR.m[2][1] + mBoxExtents.z*mAR.m[0][1];
    mBB_6 = mBoxExtents.x*mAR.m[1][1] + mBoxExtents.y*mAR.m[0][1];
    mBB_7 = mBoxExtents.y*mAR.m[2][2] + mBoxExtents.z*mAR.m[1][2];
    mBB_8 = mBoxExtents.x*mAR.m[2][2] + mBoxExtents.z*mAR.m[0][2];
    mBB_9 = mBoxExtents.x*mAR.m[1][2] + mBoxExtents.y*mAR.m[0][2];

    return FALSE;
}


inline_ bool OBBCollider::OBBContainsBox(const Point& bc, const Point& be)
{
    // I assume if all 8 box vertices are inside the OBB, so does the whole box.
    // Sounds ok but maybe there's a better way?
/*
#define TEST_PT(a,b,c)                                                                                                              \
    p.x=a;  p.y=b;  p.z=c;      p+=bc;                                                                                              \
    f = p.x * mRModelToBox.m[0][0] + p.y * mRModelToBox.m[1][0] + p.z * mRModelToBox.m[2][0];   if(f>mB0.x || f<mB1.x) return FALSE;\
    f = p.x * mRModelToBox.m[0][1] + p.y * mRModelToBox.m[1][1] + p.z * mRModelToBox.m[2][1];   if(f>mB0.y || f<mB1.y) return FALSE;\
    f = p.x * mRModelToBox.m[0][2] + p.y * mRModelToBox.m[1][2] + p.z * mRModelToBox.m[2][2];   if(f>mB0.z || f<mB1.z) return FALSE;

    Point p;
    float f;

    TEST_PT(be.x, be.y, be.z)
    TEST_PT(-be.x, be.y, be.z)
    TEST_PT(be.x, -be.y, be.z)
    TEST_PT(-be.x, -be.y, be.z)
    TEST_PT(be.x, be.y, -be.z)
    TEST_PT(-be.x, be.y, -be.z)
    TEST_PT(be.x, -be.y, -be.z)
    TEST_PT(-be.x, -be.y, -be.z)

    return TRUE;
*/

    // Yes there is:
    // - compute model-box's AABB in OBB space
    // - test AABB-in-AABB
    float NCx = bc.x * mRModelToBox.m[0][0] + bc.y * mRModelToBox.m[1][0] + bc.z * mRModelToBox.m[2][0];
    float NEx = fabsf(mRModelToBox.m[0][0] * be.x) + fabsf(mRModelToBox.m[1][0] * be.y) + fabsf(mRModelToBox.m[2][0] * be.z);

    if(mB0.x < NCx+NEx) return FALSE;
    if(mB1.x > NCx-NEx) return FALSE;

    float NCy = bc.x * mRModelToBox.m[0][1] + bc.y * mRModelToBox.m[1][1] + bc.z * mRModelToBox.m[2][1];
    float NEy = fabsf(mRModelToBox.m[0][1] * be.x) + fabsf(mRModelToBox.m[1][1] * be.y) + fabsf(mRModelToBox.m[2][1] * be.z);

    if(mB0.y < NCy+NEy) return FALSE;
    if(mB1.y > NCy-NEy) return FALSE;

    float NCz = bc.x * mRModelToBox.m[0][2] + bc.y * mRModelToBox.m[1][2] + bc.z * mRModelToBox.m[2][2];
    float NEz = fabsf(mRModelToBox.m[0][2] * be.x) + fabsf(mRModelToBox.m[1][2] * be.y) + fabsf(mRModelToBox.m[2][2] * be.z);

    if(mB0.z < NCz+NEz) return FALSE;
    if(mB1.z > NCz-NEz) return FALSE;

    return TRUE;
}

#define TEST_BOX_IN_OBB(center, extents)    \
    if(OBBContainsBox(center, extents))     \
    {                                       \
        /* Set contact status */            \
        mFlags |= OPC_CONTACT;              \
        _Dump(node);                        \
        return;                             \
    }


void OBBCollider::_Collide(const AABBCollisionNode* node)
{
    // Perform OBB-AABB overlap test
    if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter))   return;

    TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents)

    if(node->IsLeaf())
    {
        OBB_PRIM(node->GetPrimitive(), OPC_CONTACT)
    }
    else
    {
        _Collide(node->GetPos());

        if(ContactFound()) return;

        _Collide(node->GetNeg());
    }
}


void OBBCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node)
{
    // Perform OBB-AABB overlap test
    if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter))   return;

    TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents)

    if(node->IsLeaf())
    {
        SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
    }
    else
    {
        _CollideNoPrimitiveTest(node->GetPos());

        if(ContactFound()) return;

        _CollideNoPrimitiveTest(node->GetNeg());
    }
}


void OBBCollider::_Collide(const AABBQuantizedNode* node)
{
    // Dequantize box
    const QuantizedAABB& Box = node->mAABB;
    const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
    const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);

    // Perform OBB-AABB overlap test
    if(!BoxBoxOverlap(Extents, Center)) return;

    TEST_BOX_IN_OBB(Center, Extents)

    if(node->IsLeaf())
    {
        OBB_PRIM(node->GetPrimitive(), OPC_CONTACT)
    }
    else
    {
        _Collide(node->GetPos());

        if(ContactFound()) return;

        _Collide(node->GetNeg());
    }
}


void OBBCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node)
{
    // Dequantize box
    const QuantizedAABB& Box = node->mAABB;
    const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
    const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);

    // Perform OBB-AABB overlap test
    if(!BoxBoxOverlap(Extents, Center)) return;

    TEST_BOX_IN_OBB(Center, Extents)

    if(node->IsLeaf())
    {
        SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
    }
    else
    {
        _CollideNoPrimitiveTest(node->GetPos());

        if(ContactFound()) return;

        _CollideNoPrimitiveTest(node->GetNeg());
    }
}


void OBBCollider::_Collide(const AABBNoLeafNode* node)
{
    // Perform OBB-AABB overlap test
    if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter))   return;

    TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents)

    if(node->HasPosLeaf())  { OBB_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
    else                    _Collide(node->GetPos());

    if(ContactFound()) return;

    if(node->HasNegLeaf())  { OBB_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
    else                    _Collide(node->GetNeg());
}


void OBBCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node)
{
    // Perform OBB-AABB overlap test
    if(!BoxBoxOverlap(node->mAABB.mExtents, node->mAABB.mCenter))   return;

    TEST_BOX_IN_OBB(node->mAABB.mCenter, node->mAABB.mExtents)

    if(node->HasPosLeaf())  { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
    else                    _CollideNoPrimitiveTest(node->GetPos());

    if(ContactFound()) return;

    if(node->HasNegLeaf())  { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
    else                    _CollideNoPrimitiveTest(node->GetNeg());
}


void OBBCollider::_Collide(const AABBQuantizedNoLeafNode* node)
{
    // Dequantize box
    const QuantizedAABB& Box = node->mAABB;
    const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
    const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);

    // Perform OBB-AABB overlap test
    if(!BoxBoxOverlap(Extents, Center)) return;

    TEST_BOX_IN_OBB(Center, Extents)

    if(node->HasPosLeaf())  { OBB_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
    else                    _Collide(node->GetPos());

    if(ContactFound()) return;

    if(node->HasNegLeaf())  { OBB_PRIM(node->GetNegPrimitive(), OPC_CONTACT) }
    else                    _Collide(node->GetNeg());
}


void OBBCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node)
{
    // Dequantize box
    const QuantizedAABB& Box = node->mAABB;
    const Point Center(float(Box.mCenter[0]) * mCenterCoeff.x, float(Box.mCenter[1]) * mCenterCoeff.y, float(Box.mCenter[2]) * mCenterCoeff.z);
    const Point Extents(float(Box.mExtents[0]) * mExtentsCoeff.x, float(Box.mExtents[1]) * mExtentsCoeff.y, float(Box.mExtents[2]) * mExtentsCoeff.z);

    // Perform OBB-AABB overlap test
    if(!BoxBoxOverlap(Extents, Center)) return;

    TEST_BOX_IN_OBB(Center, Extents)

    if(node->HasPosLeaf())  { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
    else                    _CollideNoPrimitiveTest(node->GetPos());

    if(ContactFound()) return;

    if(node->HasNegLeaf())  { SET_CONTACT(node->GetNegPrimitive(), OPC_CONTACT) }
    else                    _CollideNoPrimitiveTest(node->GetNeg());
}







HybridOBBCollider::HybridOBBCollider()
{
}


HybridOBBCollider::~HybridOBBCollider()
{
}

bool HybridOBBCollider::Collide(OBBCache& cache, const OBB& box, const HybridModel& model, const Matrix4x4* worldb, const Matrix4x4* worldm)
{
    // We don't want primitive tests here!
    mFlags |= OPC_NO_PRIMITIVE_TESTS;

    // Checkings
    if(!Setup(&model))  return false;

    // Init collision query
    if(InitQuery(cache, box, worldb, worldm))   return true;

    // Special case for 1-leaf trees
    if(mCurrentModel && mCurrentModel->HasSingleNode())
    {
        // Here we're supposed to perform a normal query, except our tree has a single node, i.e. just a few triangles
        udword Nb = mIMesh->GetNbTriangles();

        // Loop through all triangles
        for(udword i=0;i<Nb;i++)
        {
            OBB_PRIM(i, OPC_CONTACT)
        }
        return true;
    }

    // Override destination array since we're only going to get leaf boxes here
    mTouchedBoxes.Reset();
    mTouchedPrimitives = &mTouchedBoxes;

    // Now, do the actual query against leaf boxes
    if(!model.HasLeafNodes())
    {
        if(model.IsQuantized())
        {
            const AABBQuantizedNoLeafTree* Tree = (const AABBQuantizedNoLeafTree*)model.GetTree();

            // Setup dequantization coeffs
            mCenterCoeff    = Tree->mCenterCoeff;
            mExtentsCoeff   = Tree->mExtentsCoeff;

            // Perform collision query - we don't want primitive tests here!
            _CollideNoPrimitiveTest(Tree->GetNodes());
        }
        else
        {
            const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();

            // Perform collision query - we don't want primitive tests here!
            _CollideNoPrimitiveTest(Tree->GetNodes());
        }
    }
    else
    {
        if(model.IsQuantized())
        {
            const AABBQuantizedTree* Tree = (const AABBQuantizedTree*)model.GetTree();

            // Setup dequantization coeffs
            mCenterCoeff    = Tree->mCenterCoeff;
            mExtentsCoeff   = Tree->mExtentsCoeff;

            // Perform collision query - we don't want primitive tests here!
            _CollideNoPrimitiveTest(Tree->GetNodes());
        }
        else
        {
            const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();

            // Perform collision query - we don't want primitive tests here!
            _CollideNoPrimitiveTest(Tree->GetNodes());
        }
    }

    // We only have a list of boxes so far
    if(GetContactStatus())
    {
        // Reset contact status, since it currently only reflects collisions with leaf boxes
        Collider::InitQuery();

        // Change dest container so that we can use built-in overlap tests and get collided primitives
        cache.TouchedPrimitives.Reset();
        mTouchedPrimitives = &cache.TouchedPrimitives;

        // Read touched leaf boxes
        udword Nb = mTouchedBoxes.GetNbEntries();
        const udword* Touched = mTouchedBoxes.GetEntries();

        const LeafTriangles* LT = model.GetLeafTriangles();
        const udword* Indices = model.GetIndices();

        // Loop through touched leaves
        while(Nb--)
        {
            const LeafTriangles& CurrentLeaf = LT[*Touched++];

            // Each leaf box has a set of triangles
            udword NbTris = CurrentLeaf.GetNbTriangles();
            if(Indices)
            {
                const udword* T = &Indices[CurrentLeaf.GetTriangleIndex()];

                // Loop through triangles and test each of them
                while(NbTris--)
                {
                    udword TriangleIndex = *T++;
                    OBB_PRIM(TriangleIndex, OPC_CONTACT)
                }
            }
            else
            {
                udword BaseIndex = CurrentLeaf.GetTriangleIndex();

                // Loop through triangles and test each of them
                while(NbTris--)
                {
                    udword TriangleIndex = BaseIndex++;
                    OBB_PRIM(TriangleIndex, OPC_CONTACT)
                }
            }
        }
    }

    return true;
}