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);
#if 1 // Added by AIST
        linear[box_id].mAABB.CreateSSV();
#endif
        // Store remaining info
        if(current_node->IsLeaf())
        {
                // The input tree must be complete => i.e. one primitive/leaf
                ASSERT(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 = (EXWORD)&linear[PosID];
                // Make sure it's not marked as leaf
                ASSERT(!(linear[box_id].mData&1));
                
                //Modified by S-cubed, Inc.
                //examine_normal_vector@InsertCollisionPair.cpp $B$G?F$rDI$($k$h$&$K$9$k(B
                ((AABBCollisionNode*)linear[box_id].mData)->mB     = &linear[box_id];
                (((AABBCollisionNode*)linear[box_id].mData)+1)->mB = &linear[box_id];
                // 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?!
        ASSERT(P);
        ASSERT(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
                ASSERT(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 = (EXWORD)&linear[PosID];
                // Make sure it's not marked as leaf
                ASSERT(!(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
                ASSERT(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 = (EXWORD)&linear[NegID];
                // Make sure it's not marked as leaf
                ASSERT(!(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;

        // Modified by S-cubed, Inc.
        //$B%k!<%H$N?F$O<+J,<+?H(B
        mNodes[0].mB = &mNodes[0];

        _BuildCollisionTree(mNodes, 0, CurID, tree);
        ASSERT(CurID==mNbNodes);

        return true;
}



bool AABBCollisionTree::Refit(const MeshInterface* mesh_interface)
{
        ASSERT(!"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);
        ASSERT(CurID==mNbNodes);

        return true;
}

inline_ void ComputeMinMax(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());
                        ComputeMinMax(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());
                        ComputeMinMax(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(udword 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 */                                                                                 \
                        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);                                                                                                                  \
                }                                                                                                                                                       \
        }

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



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);

                // Get max values
                FIND_MAX_VALUES

                // Quantization
                INIT_QUANTIZATION

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

                DELETEARRAY(Nodes);
        }

        return true;
}



bool AABBQuantizedTree::Refit(const MeshInterface* mesh_interface)
{
        ASSERT(!"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);
        ASSERT(CurID==mNbNodes);

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

                // Get max values
                FIND_MAX_VALUES

                // Quantization
                INIT_QUANTIZATION

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

                DELETEARRAY(Nodes);
        }

        return true;
}



bool AABBQuantizedNoLeafTree::Refit(const MeshInterface* mesh_interface)
{
        ASSERT(!"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;
}