OPC_PlanesCollider.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_PlanesAABBOverlap.h"
#include "OPC_PlanesTriOverlap.h"

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

#define PLANES_PRIM(prim_index, flag)       \
    /* Request vertices from the app */     \
    mIMesh->GetTriangle(mVP, prim_index);   \
    /* Perform triangle-box overlap test */ \
    if(PlanesTriOverlap(clip_mask))         \
    {                                       \
        SET_CONTACT(prim_index, flag)       \
    }


PlanesCollider::PlanesCollider() :
    mNbPlanes   (0),
    mPlanes     (null)
{
}


PlanesCollider::~PlanesCollider()
{
    DELETEARRAY(mPlanes);
}


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

    return VolumeCollider::ValidateSettings();
}


bool PlanesCollider::Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, const Model& model, const Matrix4x4* worldm)
{
    // Checkings
    if(!Setup(&model))  return false;

    // Init collision query
    if(InitQuery(cache, planes, nb_planes, worldm)) return true;

    udword PlaneMask = (1<<nb_planes)-1;

    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(), PlaneMask);
            else                        _Collide(Tree->GetNodes(), PlaneMask);
        }
        else
        {
            const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();

            // Perform collision query
            if(SkipPrimitiveTests())    _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
            else                        _Collide(Tree->GetNodes(), PlaneMask);
        }
    }
    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(), PlaneMask);
            else                        _Collide(Tree->GetNodes(), PlaneMask);
        }
        else
        {
            const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();

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


bool PlanesCollider::InitQuery(PlanesCache& cache, const Plane* planes, udword nb_planes, const Matrix4x4* worldm)
{
    // 1) Call the base method
    VolumeCollider::InitQuery();

    // 2) Compute planes in model space
    if(nb_planes>mNbPlanes)
    {
        DELETEARRAY(mPlanes);
        mPlanes = new Plane[nb_planes];
    }
    mNbPlanes = nb_planes;

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

//      for(udword i=0;i<nb_planes;i++) mPlanes[i] = planes[i] * InvWorldM;
        for(udword i=0;i<nb_planes;i++) TransformPlane(mPlanes[i], planes[i], InvWorldM);
    }
    else CopyMemory(mPlanes, planes, nb_planes*sizeof(Plane));

    // 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 planes (and set contact status if needed)
            udword clip_mask = (1<<mNbPlanes)-1;
            PLANES_PRIM(udword(0), OPC_CONTACT)

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

    // 4) 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 planes (and set contact status if needed)
                udword clip_mask = (1<<mNbPlanes)-1;
                PLANES_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 mTouchedPrimitives->Reset();
    }
    else
    {
        // Here we don't use temporal coherence => do a normal query
        mTouchedPrimitives->Reset();
    }

    return FALSE;
}

#define TEST_CLIP_MASK                                                                                  \
    /* If the box is completely included, so are its children. We don't need to do extra tests, we */   \
    /* can immediately output a list of visible children. Those ones won't need to be clipped. */       \
    if(!OutClipMask)                                                                                    \
    {                                                                                                   \
        /* Set contact status */                                                                        \
        mFlags |= OPC_CONTACT;                                                                          \
        _Dump(node);                                                                                    \
        return;                                                                                         \
    }


void PlanesCollider::_Collide(const AABBCollisionNode* node, udword clip_mask)
{
    // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
    udword OutClipMask;
    if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask))   return;

    TEST_CLIP_MASK

    // Else the box straddles one or several planes, so we need to recurse down the tree.
    if(node->IsLeaf())
    {
        PLANES_PRIM(node->GetPrimitive(), OPC_CONTACT)
    }
    else
    {
        _Collide(node->GetPos(), OutClipMask);

        if(ContactFound()) return;

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


void PlanesCollider::_CollideNoPrimitiveTest(const AABBCollisionNode* node, udword clip_mask)
{
    // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
    udword OutClipMask;
    if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask))   return;

    TEST_CLIP_MASK

    // Else the box straddles one or several planes, so we need to recurse down the tree.
    if(node->IsLeaf())
    {
        SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
    }
    else
    {
        _CollideNoPrimitiveTest(node->GetPos(), OutClipMask);

        if(ContactFound()) return;

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


void PlanesCollider::_Collide(const AABBQuantizedNode* node, udword clip_mask)
{
    // 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);

    // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
    udword OutClipMask;
    if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask)) return;

    TEST_CLIP_MASK

    // Else the box straddles one or several planes, so we need to recurse down the tree.
    if(node->IsLeaf())
    {
        PLANES_PRIM(node->GetPrimitive(), OPC_CONTACT)
    }
    else
    {
        _Collide(node->GetPos(), OutClipMask);

        if(ContactFound()) return;

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


void PlanesCollider::_CollideNoPrimitiveTest(const AABBQuantizedNode* node, udword clip_mask)
{
    // 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);

    // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
    udword OutClipMask;
    if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask)) return;

    TEST_CLIP_MASK

    // Else the box straddles one or several planes, so we need to recurse down the tree.
    if(node->IsLeaf())
    {
        SET_CONTACT(node->GetPrimitive(), OPC_CONTACT)
    }
    else
    {
        _CollideNoPrimitiveTest(node->GetPos(), OutClipMask);

        if(ContactFound()) return;

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


void PlanesCollider::_Collide(const AABBNoLeafNode* node, udword clip_mask)
{
    // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
    udword OutClipMask;
    if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask))   return;

    TEST_CLIP_MASK

    // Else the box straddles one or several planes, so we need to recurse down the tree.
    if(node->HasPosLeaf())  { PLANES_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
    else                    _Collide(node->GetPos(), OutClipMask);

    if(ContactFound()) return;

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


void PlanesCollider::_CollideNoPrimitiveTest(const AABBNoLeafNode* node, udword clip_mask)
{
    // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
    udword OutClipMask;
    if(!PlanesAABBOverlap(node->mAABB.mCenter, node->mAABB.mExtents, OutClipMask, clip_mask))   return;

    TEST_CLIP_MASK

    // Else the box straddles one or several planes, so we need to recurse down the tree.
    if(node->HasPosLeaf())  { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
    else                    _CollideNoPrimitiveTest(node->GetPos(), OutClipMask);

    if(ContactFound()) return;

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


void PlanesCollider::_Collide(const AABBQuantizedNoLeafNode* node, udword clip_mask)
{
    // 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);

    // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
    udword OutClipMask;
    if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask)) return;

    TEST_CLIP_MASK

    // Else the box straddles one or several planes, so we need to recurse down the tree.
    if(node->HasPosLeaf())  { PLANES_PRIM(node->GetPosPrimitive(), OPC_CONTACT) }
    else                    _Collide(node->GetPos(), OutClipMask);

    if(ContactFound()) return;

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


void PlanesCollider::_CollideNoPrimitiveTest(const AABBQuantizedNoLeafNode* node, udword clip_mask)
{
    // 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);

    // Test the box against the planes. If the box is completely culled, so are its children, hence we exit.
    udword OutClipMask;
    if(!PlanesAABBOverlap(Center, Extents, OutClipMask, clip_mask)) return;

    TEST_CLIP_MASK

    // Else the box straddles one or several planes, so we need to recurse down the tree.
    if(node->HasPosLeaf())  { SET_CONTACT(node->GetPosPrimitive(), OPC_CONTACT) }
    else                    _CollideNoPrimitiveTest(node->GetPos(), OutClipMask);

    if(ContactFound()) return;

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








HybridPlanesCollider::HybridPlanesCollider()
{
}


HybridPlanesCollider::~HybridPlanesCollider()
{
}

bool HybridPlanesCollider::Collide(PlanesCache& cache, const Plane* planes, udword nb_planes, const HybridModel& model, 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, planes, nb_planes, 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
        udword clip_mask = (1<<mNbPlanes)-1;
        for(udword i=0;i<Nb;i++)
        {
            PLANES_PRIM(i, OPC_CONTACT)
        }
        return true;
    }

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

    udword PlaneMask = (1<<nb_planes)-1;

    // 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(), PlaneMask);
        }
        else
        {
            const AABBNoLeafTree* Tree = (const AABBNoLeafTree*)model.GetTree();

            // Perform collision query - we don't want primitive tests here!
            _CollideNoPrimitiveTest(Tree->GetNodes(), PlaneMask);
        }
    }
    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(), PlaneMask);
        }
        else
        {
            const AABBCollisionTree* Tree = (const AABBCollisionTree*)model.GetTree();

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

    // 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
        udword clip_mask = (1<<mNbPlanes)-1;
        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++;
                    PLANES_PRIM(TriangleIndex, OPC_CONTACT)
                }
            }
            else
            {
                udword BaseIndex = CurrentLeaf.GetTriangleIndex();

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

    return true;
}