OPC_OptimizedTree.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;


static bool gFixQuantized = true;


static void _BuildCollisionTree(AABBCollisionNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node)
{
    // Current node from input tree is "current_node". Must be flattened into "linear[boxid]".

    // Store the AABB
    current_node->GetAABB()->GetCenter(linear[box_id].mAABB.mCenter);
    current_node->GetAABB()->GetExtents(linear[box_id].mAABB.mExtents);
    // Store remaining info
    if(current_node->IsLeaf())
    {
        // The input tree must be complete => i.e. one primitive/leaf
        OPASSERT(current_node->GetNbPrimitives()==1);
        // Get the primitive index from the input tree
        udword PrimitiveIndex = current_node->GetPrimitives()[0];
        // Setup box data as the primitive index, marked as leaf
        linear[box_id].mData = (PrimitiveIndex<<1)|1;
    }
    else
    {
        // To make the negative one implicit, we must store P and N in successive order
        udword PosID = current_id++;    // Get a new id for positive child
        udword NegID = current_id++;    // Get a new id for negative child
        // Setup box data as the forthcoming new P pointer
        linear[box_id].mData = (uintptr_t)&linear[PosID];
        // Make sure it's not marked as leaf
        OPASSERT(!(linear[box_id].mData&1));
        // Recurse with new IDs
        _BuildCollisionTree(linear, PosID, current_id, current_node->GetPos());
        _BuildCollisionTree(linear, NegID, current_id, current_node->GetNeg());
    }
}


static void _BuildNoLeafTree(AABBNoLeafNode* linear, const udword box_id, udword& current_id, const AABBTreeNode* current_node)
{
    const AABBTreeNode* P = current_node->GetPos();
    const AABBTreeNode* N = current_node->GetNeg();
    // Leaf nodes here?!
    OPASSERT(P);
    OPASSERT(N);
    // Internal node => keep the box
    current_node->GetAABB()->GetCenter(linear[box_id].mAABB.mCenter);
    current_node->GetAABB()->GetExtents(linear[box_id].mAABB.mExtents);

    if(P->IsLeaf())
    {
        // The input tree must be complete => i.e. one primitive/leaf
        OPASSERT(P->GetNbPrimitives()==1);
        // Get the primitive index from the input tree
        udword PrimitiveIndex = P->GetPrimitives()[0];
        // Setup prev box data as the primitive index, marked as leaf
        linear[box_id].mPosData = (PrimitiveIndex<<1)|1;
    }
    else
    {
        // Get a new id for positive child
        udword PosID = current_id++;
        // Setup box data
        linear[box_id].mPosData = (uintptr_t)&linear[PosID];
        // Make sure it's not marked as leaf
        OPASSERT(!(linear[box_id].mPosData&1));
        // Recurse
        _BuildNoLeafTree(linear, PosID, current_id, P);
    }

    if(N->IsLeaf())
    {
        // The input tree must be complete => i.e. one primitive/leaf
        OPASSERT(N->GetNbPrimitives()==1);
        // Get the primitive index from the input tree
        udword PrimitiveIndex = N->GetPrimitives()[0];
        // Setup prev box data as the primitive index, marked as leaf
        linear[box_id].mNegData = (PrimitiveIndex<<1)|1;
    }
    else
    {
        // Get a new id for negative child
        udword NegID = current_id++;
        // Setup box data
        linear[box_id].mNegData = (uintptr_t)&linear[NegID];
        // Make sure it's not marked as leaf
        OPASSERT(!(linear[box_id].mNegData&1));
        // Recurse
        _BuildNoLeafTree(linear, NegID, current_id, N);
    }
}


AABBCollisionTree::AABBCollisionTree() : mNodes(null)
{
}


AABBCollisionTree::~AABBCollisionTree()
{
    DELETEARRAY(mNodes);
}


bool AABBCollisionTree::Build(AABBTree* tree)
{
    // Checkings
    if(!tree)   return false;
    // Check the input tree is complete
    udword NbTriangles  = tree->GetNbPrimitives();
    udword NbNodes      = tree->GetNbNodes();
    if(NbNodes!=NbTriangles*2-1)    return false;

    // Get nodes
    if(mNbNodes!=NbNodes)   // Same number of nodes => keep moving
    {
        mNbNodes = NbNodes;
        DELETEARRAY(mNodes);
        mNodes = new AABBCollisionNode[mNbNodes];
        CHECKALLOC(mNodes);
    }

    // Build the tree
    udword CurID = 1;
    _BuildCollisionTree(mNodes, 0, CurID, tree);
    OPASSERT(CurID==mNbNodes);

    return true;
}


bool AABBCollisionTree::Refit(const MeshInterface* /*mesh_interface*/)
{
    OPASSERT(!"Not implemented since AABBCollisionTrees have twice as more nodes to refit as AABBNoLeafTrees!");
    return false;
}


bool AABBCollisionTree::Walk(GenericWalkingCallback callback, void* user_data) const
{
    if(!callback)   return false;

    struct Local
    {
        static void _Walk(const AABBCollisionNode* current_node, GenericWalkingCallback callback, void* user_data)
        {
            if(!current_node || !(callback)(current_node, user_data))   return;

            if(!current_node->IsLeaf())
            {
                _Walk(current_node->GetPos(), callback, user_data);
                _Walk(current_node->GetNeg(), callback, user_data);
            }
        }
    };
    Local::_Walk(mNodes, callback, user_data);
    return true;
}



AABBNoLeafTree::AABBNoLeafTree() : mNodes(null)
{
}


AABBNoLeafTree::~AABBNoLeafTree()
{
    DELETEARRAY(mNodes);
}


bool AABBNoLeafTree::Build(AABBTree* tree)
{
    // Checkings
    if(!tree)   return false;
    // Check the input tree is complete
    udword NbTriangles  = tree->GetNbPrimitives();
    udword NbNodes      = tree->GetNbNodes();
    if(NbNodes!=NbTriangles*2-1)    return false;

    // Get nodes
    if(mNbNodes!=NbTriangles-1) // Same number of nodes => keep moving
    {
        mNbNodes = NbTriangles-1;
        DELETEARRAY(mNodes);
        mNodes = new AABBNoLeafNode[mNbNodes];
        CHECKALLOC(mNodes);
    }

    // Build the tree
    udword CurID = 1;
    _BuildNoLeafTree(mNodes, 0, CurID, tree);
    OPASSERT(CurID==mNbNodes);

    return true;
}

inline_ void OPComputeMinMax(Point& min, Point& max, const VertexPointers& vp)
{
    // Compute triangle's AABB = a leaf box
#ifdef OPC_USE_FCOMI    // a 15% speedup on my machine, not much
    min.x = FCMin3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x);
    max.x = FCMax3(vp.Vertex[0]->x, vp.Vertex[1]->x, vp.Vertex[2]->x);

    min.y = FCMin3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y);
    max.y = FCMax3(vp.Vertex[0]->y, vp.Vertex[1]->y, vp.Vertex[2]->y);

    min.z = FCMin3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z);
    max.z = FCMax3(vp.Vertex[0]->z, vp.Vertex[1]->z, vp.Vertex[2]->z);
#else
    min = *vp.Vertex[0];
    max = *vp.Vertex[0];
    min.Min(*vp.Vertex[1]);
    max.Max(*vp.Vertex[1]);
    min.Min(*vp.Vertex[2]);
    max.Max(*vp.Vertex[2]);
#endif
}


bool AABBNoLeafTree::Refit(const MeshInterface* mesh_interface)
{
    // Checkings
    if(!mesh_interface) return false;

    // Bottom-up update
    VertexPointers VP;
    Point Min,Max;
    Point Min_,Max_;
    udword Index = mNbNodes;
    while(Index--)
    {
        AABBNoLeafNode& Current = mNodes[Index];

        if(Current.HasPosLeaf())
        {
            mesh_interface->GetTriangle(VP, Current.GetPosPrimitive());
            OPComputeMinMax(Min, Max, VP);
        }
        else
        {
            const CollisionAABB& CurrentBox = Current.GetPos()->mAABB;
            CurrentBox.GetMin(Min);
            CurrentBox.GetMax(Max);
        }

        if(Current.HasNegLeaf())
        {
            mesh_interface->GetTriangle(VP, Current.GetNegPrimitive());
            OPComputeMinMax(Min_, Max_, VP);
        }
        else
        {
            const CollisionAABB& CurrentBox = Current.GetNeg()->mAABB;
            CurrentBox.GetMin(Min_);
            CurrentBox.GetMax(Max_);
        }
#ifdef OPC_USE_FCOMI
        Min.x = FCMin2(Min.x, Min_.x);
        Max.x = FCMax2(Max.x, Max_.x);
        Min.y = FCMin2(Min.y, Min_.y);
        Max.y = FCMax2(Max.y, Max_.y);
        Min.z = FCMin2(Min.z, Min_.z);
        Max.z = FCMax2(Max.z, Max_.z);
#else
        Min.Min(Min_);
        Max.Max(Max_);
#endif
        Current.mAABB.SetMinMax(Min, Max);
    }
    return true;
}


bool AABBNoLeafTree::Walk(GenericWalkingCallback callback, void* user_data) const
{
    if(!callback)   return false;

    struct Local
    {
        static void _Walk(const AABBNoLeafNode* current_node, GenericWalkingCallback callback, void* user_data)
        {
            if(!current_node || !(callback)(current_node, user_data))   return;

            if(!current_node->HasPosLeaf()) _Walk(current_node->GetPos(), callback, user_data);
            if(!current_node->HasNegLeaf()) _Walk(current_node->GetNeg(), callback, user_data);
        }
    };
    Local::_Walk(mNodes, callback, user_data);
    return true;
}

// Quantization notes:
// - We could use the highest bits of mData to store some more quantized bits. Dequantization code
//   would be slightly more complex, but number of overlap tests would be reduced (and anyhow those
//   bits are currently wasted). Of course it's not possible if we move to 16 bits mData.
// - Something like "16 bits floats" could be tested, to bypass the int-to-float conversion.
// - A dedicated BV-BV test could be used, dequantizing while testing for overlap. (i.e. it's some
//   lazy-dequantization which may save some work in case of early exits). At the very least some
//   muls could be saved by precomputing several more matrices. But maybe not worth the pain.
// - Do we need to dequantize anyway? Not doing the extents-related muls only implies the box has
//   been scaled, for example.
// - The deeper we move into the hierarchy, the smaller the extents should be. May not need a fixed
//   number of quantization bits. Even better, could probably be best delta-encoded.


// Find max values. Some people asked why I wasn't simply using the first node. Well, I can't.
// I'm not looking for (min, max) values like in a standard AABB, I'm looking for the extremal
// centers/extents in order to quantize them. The first node would only give a single center and
// a single extents. While extents would be the biggest, the center wouldn't.
#define FIND_MAX_VALUES                                                                         \
    /* Get max values */                                                                        \
    Point CMax(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT);                                                \
    Point EMax(MIN_FLOAT, MIN_FLOAT, MIN_FLOAT);                                                \
    for(i=0;i<mNbNodes;i++)                                                             \
    {                                                                                           \
        if(fabsf(Nodes[i].mAABB.mCenter.x)>CMax.x)  CMax.x = fabsf(Nodes[i].mAABB.mCenter.x);   \
        if(fabsf(Nodes[i].mAABB.mCenter.y)>CMax.y)  CMax.y = fabsf(Nodes[i].mAABB.mCenter.y);   \
        if(fabsf(Nodes[i].mAABB.mCenter.z)>CMax.z)  CMax.z = fabsf(Nodes[i].mAABB.mCenter.z);   \
        if(fabsf(Nodes[i].mAABB.mExtents.x)>EMax.x) EMax.x = fabsf(Nodes[i].mAABB.mExtents.x);  \
        if(fabsf(Nodes[i].mAABB.mExtents.y)>EMax.y) EMax.y = fabsf(Nodes[i].mAABB.mExtents.y);  \
        if(fabsf(Nodes[i].mAABB.mExtents.z)>EMax.z) EMax.z = fabsf(Nodes[i].mAABB.mExtents.z);  \
    }

#define INIT_QUANTIZATION                                                   \
    udword nbc=15;  /* Keep one bit for sign */                             \
    udword nbe=15;  /* Keep one bit for fix */                              \
    if(!gFixQuantized) nbe++;                                               \
                                                                            \
    /* Compute quantization coeffs */                                       \
    Point CQuantCoeff, EQuantCoeff;                                         \
    CQuantCoeff.x = CMax.x!=0.0f ? float((1<<nbc)-1)/CMax.x : 0.0f;         \
    CQuantCoeff.y = CMax.y!=0.0f ? float((1<<nbc)-1)/CMax.y : 0.0f;         \
    CQuantCoeff.z = CMax.z!=0.0f ? float((1<<nbc)-1)/CMax.z : 0.0f;         \
    EQuantCoeff.x = EMax.x!=0.0f ? float((1<<nbe)-1)/EMax.x : 0.0f;         \
    EQuantCoeff.y = EMax.y!=0.0f ? float((1<<nbe)-1)/EMax.y : 0.0f;         \
    EQuantCoeff.z = EMax.z!=0.0f ? float((1<<nbe)-1)/EMax.z : 0.0f;         \
    /* Compute and save dequantization coeffs */                            \
    mCenterCoeff.x = CQuantCoeff.x!=0.0f ? 1.0f / CQuantCoeff.x : 0.0f;     \
    mCenterCoeff.y = CQuantCoeff.y!=0.0f ? 1.0f / CQuantCoeff.y : 0.0f;     \
    mCenterCoeff.z = CQuantCoeff.z!=0.0f ? 1.0f / CQuantCoeff.z : 0.0f;     \
    mExtentsCoeff.x = EQuantCoeff.x!=0.0f ? 1.0f / EQuantCoeff.x : 0.0f;    \
    mExtentsCoeff.y = EQuantCoeff.y!=0.0f ? 1.0f / EQuantCoeff.y : 0.0f;    \
    mExtentsCoeff.z = EQuantCoeff.z!=0.0f ? 1.0f / EQuantCoeff.z : 0.0f;    \

#define PERFORM_QUANTIZATION                                                        \
    /* Quantize */                                                                  \
    mNodes[i].mAABB.mCenter[0] = sword(Nodes[i].mAABB.mCenter.x * CQuantCoeff.x);   \
    mNodes[i].mAABB.mCenter[1] = sword(Nodes[i].mAABB.mCenter.y * CQuantCoeff.y);   \
    mNodes[i].mAABB.mCenter[2] = sword(Nodes[i].mAABB.mCenter.z * CQuantCoeff.z);   \
    mNodes[i].mAABB.mExtents[0] = uword(Nodes[i].mAABB.mExtents.x * EQuantCoeff.x); \
    mNodes[i].mAABB.mExtents[1] = uword(Nodes[i].mAABB.mExtents.y * EQuantCoeff.y); \
    mNodes[i].mAABB.mExtents[2] = uword(Nodes[i].mAABB.mExtents.z * EQuantCoeff.z); \
    /* Fix quantized boxes */                                                       \
    if(gFixQuantized)                                                               \
    {                                                                               \
        /* Make sure the quantized box is still valid */                            \
        Point Max = Nodes[i].mAABB.mCenter + Nodes[i].mAABB.mExtents;               \
        Point Min = Nodes[i].mAABB.mCenter - Nodes[i].mAABB.mExtents;               \
        /* For each axis */                                                         \
        for(udword j=0;j<3;j++)                                                     \
        {   /* Dequantize the box center */                                         \
            if (fabs (mExtentsCoeff[j]) < 0.00001) \
            { \
                mNodes[i].mAABB.mExtents[j]=0xffff; \
            } \
            else \
            { \
                float qc = float(mNodes[i].mAABB.mCenter[j]) * mCenterCoeff[j];         \
                bool FixMe=true;                                                        \
                do                                                                      \
                {   /* Dequantize the box extent */                                     \
                float qe = float(mNodes[i].mAABB.mExtents[j]) * mExtentsCoeff[j];   \
                /* Compare real & dequantized values */                             \
                if(qc+qe<Max[j] || qc-qe>Min[j])    mNodes[i].mAABB.mExtents[j]++;  \
                else                                FixMe=false;                    \
                /* Prevent wrapping */                                              \
                if(!mNodes[i].mAABB.mExtents[j])                                    \
                {                                                                   \
                    mNodes[i].mAABB.mExtents[j]=0xffff;                             \
                    FixMe=false;                                                    \
                }                                                                   \
                }while(FixMe);                                                          \
               } \
        }                                                                           \
    }

#define REMAP_DATA(member)                              \
    /* Fix data */                                  \
    Data = Nodes[i].member;                             \
    if(!(Data&1))                                   \
    {                                       \
        /* Compute box number */                        \
        udword Nb = (Data - uintptr_t(Nodes))/Nodes[i].GetNodeSize();   \
        Data = uintptr_t(&mNodes[Nb]);                      \
    }                                       \
    /* ...remapped */                               \
    mNodes[i].member = Data;


AABBQuantizedTree::AABBQuantizedTree() : mNodes(null)
{
}


AABBQuantizedTree::~AABBQuantizedTree()
{
    DELETEARRAY(mNodes);
}


bool AABBQuantizedTree::Build(AABBTree* tree)
{
    // Checkings
    if(!tree)   return false;
    // Check the input tree is complete
    udword NbTriangles  = tree->GetNbPrimitives();
    udword NbNodes      = tree->GetNbNodes();
    if(NbNodes!=NbTriangles*2-1)    return false;

    // Get nodes
    mNbNodes = NbNodes;
    DELETEARRAY(mNodes);
    AABBCollisionNode* Nodes = new AABBCollisionNode[mNbNodes];
    CHECKALLOC(Nodes);

    // Build the tree
    udword CurID = 1;
    _BuildCollisionTree(Nodes, 0, CurID, tree);

    // Quantize
    {
        mNodes = new AABBQuantizedNode[mNbNodes];
        CHECKALLOC(mNodes);

        udword i;
        // Get max values
        FIND_MAX_VALUES

        // Quantization
        INIT_QUANTIZATION

        // Quantize
        uintptr_t Data;
        for(i=0;i<mNbNodes;i++)
        {
            PERFORM_QUANTIZATION
            REMAP_DATA(mData)
        }

        DELETEARRAY(Nodes);
    }

    return true;
}


bool AABBQuantizedTree::Refit(const MeshInterface* /*mesh_interface*/)
{
    OPASSERT(!"Not implemented since requantizing is painful !");
    return false;
}


bool AABBQuantizedTree::Walk(GenericWalkingCallback callback, void* user_data) const
{
    if(!callback)   return false;

    struct Local
    {
        static void _Walk(const AABBQuantizedNode* current_node, GenericWalkingCallback callback, void* user_data)
        {
            if(!current_node || !(callback)(current_node, user_data))   return;

            if(!current_node->IsLeaf())
            {
                _Walk(current_node->GetPos(), callback, user_data);
                _Walk(current_node->GetNeg(), callback, user_data);
            }
        }
    };
    Local::_Walk(mNodes, callback, user_data);
    return true;
}




AABBQuantizedNoLeafTree::AABBQuantizedNoLeafTree() : mNodes(null)
{
}


AABBQuantizedNoLeafTree::~AABBQuantizedNoLeafTree()
{
    DELETEARRAY(mNodes);
}


bool AABBQuantizedNoLeafTree::Build(AABBTree* tree)
{
    // Checkings
    if(!tree)   return false;
    // Check the input tree is complete
    udword NbTriangles  = tree->GetNbPrimitives();
    udword NbNodes      = tree->GetNbNodes();
    if(NbNodes!=NbTriangles*2-1)    return false;

    // Get nodes
    mNbNodes = NbTriangles-1;
    DELETEARRAY(mNodes);
    AABBNoLeafNode* Nodes = new AABBNoLeafNode[mNbNodes];
    CHECKALLOC(Nodes);

    // Build the tree
    udword CurID = 1;
    _BuildNoLeafTree(Nodes, 0, CurID, tree);
    OPASSERT(CurID==mNbNodes);

    // Quantize
    {
        mNodes = new AABBQuantizedNoLeafNode[mNbNodes];
        CHECKALLOC(mNodes);

        udword i;
        // Get max values
        FIND_MAX_VALUES

        // Quantization
        INIT_QUANTIZATION

        // Quantize
        uintptr_t Data;
        for(i=0;i<mNbNodes;i++)
        {
            PERFORM_QUANTIZATION
            REMAP_DATA(mPosData)
            REMAP_DATA(mNegData)
        }

        DELETEARRAY(Nodes);
    }

    return true;
}


bool AABBQuantizedNoLeafTree::Refit(const MeshInterface* /*mesh_interface*/)
{
    OPASSERT(!"Not implemented since requantizing is painful !");
    return false;
}


bool AABBQuantizedNoLeafTree::Walk(GenericWalkingCallback callback, void* user_data) const
{
    if(!callback)   return false;

    struct Local
    {
        static void _Walk(const AABBQuantizedNoLeafNode* current_node, GenericWalkingCallback callback, void* user_data)
        {
            if(!current_node || !(callback)(current_node, user_data))   return;

            if(!current_node->HasPosLeaf()) _Walk(current_node->GetPos(), callback, user_data);
            if(!current_node->HasNegLeaf()) _Walk(current_node->GetNeg(), callback, user_data);
        }
    };
    Local::_Walk(mNodes, callback, user_data);
    return true;
}