OPC_SphereCollider.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_SphereAABBOverlap.h"
#include "OPC_SphereTriOverlap.h"

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

#define SPHERE_PRIM(prim_index, flag)                                   \
    /* Request vertices from the app */                                 \
    VertexPointers VP;  mIMesh->GetTriangle(VP, prim_index);            \
                                                                        \
    /* Perform sphere-tri overlap test */                               \
    if(SphereTriOverlap(*VP.Vertex[0], *VP.Vertex[1], *VP.Vertex[2]))   \
    {                                                                   \
        SET_CONTACT(prim_index, flag)                                   \
    }


SphereCollider::SphereCollider()
{
    mCenter.Zero();
    mRadius2 = 0.0f;
}


SphereCollider::~SphereCollider()
{
}


bool SphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const Model& model, const Matrix4x4* worlds, const Matrix4x4* worldm)
{
    // Checkings
    if(!Setup(&model))  return false;

    // Init collision query
    if(InitQuery(cache, sphere, worlds, 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 SphereCollider::InitQuery(SphereCache& cache, const Sphere& sphere, const Matrix4x4* worlds, const Matrix4x4* worldm)
{
    // 1) Call the base method
    VolumeCollider::InitQuery();

    // 2) Compute sphere in model space:
    // - Precompute R^2
    mRadius2 = sphere.mRadius * sphere.mRadius;
    // - Compute center position
    mCenter = sphere.mCenter;
    // -> to world space
    if(worlds)  mCenter *= *worlds;
    // -> to model space
    if(worldm)
    {
        // Invert model matrix
        Matrix4x4 InvWorldM;
        InvertPRMatrix(InvWorldM, *worldm);

        mCenter *= InvWorldM;
    }

    // 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 sphere (and set contact status if needed)
            SPHERE_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 sphere (and set contact status if needed)
                SPHERE_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
        {
            // We're interested in all contacts =>test the new real sphere N(ew) against the previous fat sphere P(revious):
            float r = sqrtf(cache.FatRadius2) - sphere.mRadius;
            if(IsCacheValid(cache) && cache.Center.SquareDistance(mCenter) < r*r)
            {
                // - 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 sphere so that coherence will work for subsequent frames
                mRadius2 *= cache.FatCoeff;
//              mRadius2 = (sphere.mRadius * cache.FatCoeff)*(sphere.mRadius * cache.FatCoeff);

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

    return FALSE;
}


bool SphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const AABBTree* tree)
{
    // This is typically called for a scene tree, full of -AABBs-, not full of triangles.
    // So we don't really have "primitives" to deal with. Hence it doesn't work with
    // "FirstContact" + "TemporalCoherence".
    OPASSERT( !(FirstContactEnabled() && TemporalCoherenceEnabled()) );

    // Checkings
    if(!tree)   return false;

    // Init collision query
    if(InitQuery(cache, sphere))    return true;

    // Perform collision query
    _Collide(tree);

    return true;
}


inline_ bool SphereCollider::SphereContainsBox(const Point& bc, const Point& be)
{
    // I assume if all 8 box vertices are inside the sphere, so does the whole box.
    // Sounds ok but maybe there's a better way?
    Point p;
    p.x=bc.x+be.x; p.y=bc.y+be.y; p.z=bc.z+be.z;    if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
    p.x=bc.x-be.x;                                  if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
    p.x=bc.x+be.x; p.y=bc.y-be.y;                   if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
    p.x=bc.x-be.x;                                  if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
    p.x=bc.x+be.x; p.y=bc.y+be.y; p.z=bc.z-be.z;    if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
    p.x=bc.x-be.x;                                  if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
    p.x=bc.x+be.x; p.y=bc.y-be.y;                   if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;
    p.x=bc.x-be.x;                                  if(mCenter.SquareDistance(p)>=mRadius2) return FALSE;

    return TRUE;
}

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


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

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

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

        if(ContactFound()) return;

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


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

    TEST_BOX_IN_SPHERE(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 SphereCollider::_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 Sphere-AABB overlap test
    if(!SphereAABBOverlap(Center, Extents)) return;

    TEST_BOX_IN_SPHERE(Center, Extents)

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

        if(ContactFound()) return;

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


void SphereCollider::_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 Sphere-AABB overlap test
    if(!SphereAABBOverlap(Center, Extents)) return;

    TEST_BOX_IN_SPHERE(Center, Extents)

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

        if(ContactFound()) return;

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


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

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

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

    if(ContactFound()) return;

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


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

    TEST_BOX_IN_SPHERE(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 SphereCollider::_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 Sphere-AABB overlap test
    if(!SphereAABBOverlap(Center, Extents)) return;

    TEST_BOX_IN_SPHERE(Center, Extents)

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

    if(ContactFound()) return;

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


void SphereCollider::_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 Sphere-AABB overlap test
    if(!SphereAABBOverlap(Center, Extents)) return;

    TEST_BOX_IN_SPHERE(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());
}


void SphereCollider::_Collide(const AABBTreeNode* node)
{
    // Perform Sphere-AABB overlap test
    Point Center, Extents;
    node->GetAABB()->GetCenter(Center);
    node->GetAABB()->GetExtents(Extents);
    if(!SphereAABBOverlap(Center, Extents)) return;

    if(node->IsLeaf() || SphereContainsBox(Center, Extents))
    {
        mFlags |= OPC_CONTACT;
        mTouchedPrimitives->Add(node->GetPrimitives(), node->GetNbPrimitives());
    }
    else
    {
        _Collide(node->GetPos());
        _Collide(node->GetNeg());
    }
}








HybridSphereCollider::HybridSphereCollider()
{
}


HybridSphereCollider::~HybridSphereCollider()
{
}

bool HybridSphereCollider::Collide(SphereCache& cache, const Sphere& sphere, const HybridModel& model, const Matrix4x4* worlds, 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, sphere, worlds, 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++)
        {
            SPHERE_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++;
                    SPHERE_PRIM(TriangleIndex, OPC_CONTACT)
                }
            }
            else
            {
                udword BaseIndex = CurrentLeaf.GetTriangleIndex();

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

    return true;
}