source: GTP/trunk/Lib/Vis/Preprocessing/src/TraversalTree.cpp @ 2176

#include <stack>
#include <algorithm>
#include <queue>
#include "Environment.h"
#include "Mesh.h"
#include "TraversalTree.h"
#include "ViewCell.h"
#include "Beam.h"
#include "Exporter.h"


using namespace std;

// $$JB HACK
#define KD_PVS_AREA (1e-5f)

namespace GtpVisibilityPreprocessor {

int TraversalNode::sMailId = 1;
int TraversalNode::sReservedMailboxes = 1;


inline static bool ilt(Intersectable *obj1, Intersectable *obj2)
{
        return obj1->mId < obj2->mId;
}


TraversalNode::TraversalNode(TraversalInterior *parent): 
mParent(parent), mMailbox(0)
{
}


TraversalInterior::TraversalInterior(TraversalInterior *parent): 
TraversalNode(parent), mBack(NULL), mFront(NULL) 
{
}


TraversalInterior::~TraversalInterior()
{
        // recursivly destroy children
        DEL_PTR(mFront);
        DEL_PTR(mBack);
}


bool TraversalInterior::IsLeaf() const 
{ 
        return false; 
}


void TraversalInterior::SetupChildLinks(TraversalNode *b, TraversalNode *f) 
{
        mBack = b;
        mFront = f;
        
        b->mParent = f->mParent = this;
}


void TraversalInterior::ReplaceChildLink(TraversalNode *oldChild, 
                                                                                 TraversalNode *newChild) 
{
        if (mBack == oldChild)
                mBack = newChild;
        else
                mFront = newChild;
}


TraversalLeaf::TraversalLeaf(TraversalInterior *parent, const int objects):
TraversalNode(parent)
{
        mViewCells.reserve(objects);
}


bool TraversalLeaf::IsLeaf() const 
{ 
        return true; 
}


TraversalLeaf::~TraversalLeaf()
{
}


TraversalTree::TraversalTree()
{ 
        TraversalLeaf *leaf = new TraversalLeaf(NULL, 0);
        leaf->mDepth = 0;
        mRoot = leaf;

        Environment::GetSingleton()->GetIntValue("TraversalTree.Termination.maxNodes", mTermMaxNodes);
        Environment::GetSingleton()->GetIntValue("TraversalTree.Termination.maxDepth", mTermMaxDepth);
        Environment::GetSingleton()->GetIntValue("TraversalTree.Termination.minCost", mTermMinCost);
        Environment::GetSingleton()->GetFloatValue("TraversalTree.Termination.maxCostRatio", mMaxCostRatio);
        Environment::GetSingleton()->GetFloatValue("TraversalTree.Termination.ct_div_ci", mCt_div_ci);
        Environment::GetSingleton()->GetFloatValue("TraversalTree.splitBorder", mSplitBorder);

        Environment::GetSingleton()->GetBoolValue("TraversalTree.sahUseFaces", mSahUseFaces);

        char splitType[64];
        Environment::GetSingleton()->GetStringValue("TraversalTree.splitMethod", splitType);

        splitCandidates = NULL;
        mSplitMethod = SPLIT_SPATIAL_MEDIAN;

        if (strcmp(splitType, "spatialMedian") == 0)
        {
                mSplitMethod = SPLIT_SPATIAL_MEDIAN;
        }
        else
        {
                if (strcmp(splitType, "objectMedian") == 0)
                {
                        mSplitMethod = SPLIT_OBJECT_MEDIAN;
                }
                else
                {
                        if (strcmp(splitType, "SAH") == 0)
                        {
                                mSplitMethod = SPLIT_SAH;
                        }
                        else 
                        {
                                cerr << "Wrong kd split type " << splitType << endl;
                                exit(1);
                        }
                }
        }
        cout << "Traversal Tree Split method: " << mSplitMethod << endl;
}


TraversalTree::~TraversalTree()
{
    DEL_PTR(mRoot);
}


bool TraversalTree::Construct(const ViewCellContainer &viewCells)
{
        if (!splitCandidates)
        {
                splitCandidates = new vector<SortableEntry *>;
        }

        // first construct a leaf that will get subdivide
        TraversalLeaf *leaf = static_cast<TraversalLeaf *>(mRoot);

        leaf->mViewCells = viewCells;
        mStat.nodes = 1;
        mBox.Initialize();

        ViewCellContainer::const_iterator mi;

        for ( mi = leaf->mViewCells.begin(); mi != leaf->mViewCells.end(); ++ mi) 
        {
                mBox.Include((*mi)->GetBox());
        }

        cout << "TraversalTree Root Box:" << mBox << endl;
        mRoot = Subdivide(TraversalData(leaf, mBox, 0));
        
        // remove the allocated array
        if (splitCandidates)
        {
                CLEAR_CONTAINER(*splitCandidates);
                delete splitCandidates;
        }
        
        return true;
}


TraversalNode *TraversalTree::Subdivide(const TraversalData &tdata)
{
        TraversalNode *result = NULL;

        //priority_queue<TraversalData> tStack;
        stack<TraversalData> tStack;

        tStack.push(tdata);
        AxisAlignedBox3 backBox, frontBox;

        while (!tStack.empty()) 
        {
                if (mStat.Nodes() > mTermMaxNodes) 
                {
                        while (!tStack.empty()) 
                        {
                                EvaluateLeafStats(tStack.top());
                                tStack.pop();
                        }
                        break;
                }

                TraversalData data = tStack.top();
                tStack.pop();

                TraversalLeaf *tLeaf = static_cast<TraversalLeaf *> (data.mNode);
                TraversalNode *node = SubdivideNode(tLeaf,
                                                                                        data.mBox,
                                                                                        backBox,
                                                                                        frontBox);

                if (result == NULL)
                        result = node;

                if (!node->IsLeaf()) 
                {
                        TraversalInterior *interior = static_cast<TraversalInterior *>(node);

                        // push the children on the stack
                        tStack.push(TraversalData(interior->mBack, backBox, data.mDepth + 1));
                        tStack.push(TraversalData(interior->mFront, frontBox, data.mDepth + 1));

                } 
                else 
                {
                        EvaluateLeafStats(data);
                }
        }

        return result;
}


bool TraversalTree::TerminationCriteriaMet(const TraversalLeaf *leaf)
{
        const bool criteriaMet = (
                ((int)leaf->mViewCells.size() <= mTermMinCost) ||
                 (leaf->mDepth >= mTermMaxDepth) 
                 || (GetBox(leaf).SurfaceArea() < 0.00001f)
                 );

        if (criteriaMet)
        {
                cerr << "\nOBJECTS=" << (int)leaf->mViewCells.size() << endl;
                cerr << "\nDEPTH=" << (int)leaf->mDepth << endl;
        }

        return criteriaMet;
}


int TraversalTree::SelectPlane(TraversalLeaf *leaf,
                                                           const AxisAlignedBox3 &box,
                                                           float &position)
{
        int axis = -1;

        switch (mSplitMethod)
        {
        case SPLIT_SPATIAL_MEDIAN: 
                {
                        axis = box.Size().DrivingAxis();
                        position = (box.Min()[axis] + box.Max()[axis]) * 0.5f;
                        break;
                }
        case SPLIT_SAH: 
                {
                        int objectsBack, objectsFront;
                        float costRatio = 99999;//MAX_FLOAT;

                        for (int i=0; i < 3; ++ i) 
                        {
                                float p;
                                float r = BestCostRatio(leaf,
                                                                                box,
                                                                                i,
                                                                                p,
                                                                                objectsBack,
                                                                                objectsFront);
        
                                if (r < costRatio) 
                                {
                                        costRatio = r;
                                        axis = i;
                                        position = p;
                                }
                        }

                        if (costRatio > mMaxCostRatio) 
                        {
                                cout << "Too big cost ratio " << costRatio << endl;
                                axis = -1;
                        }
                        break;
                }
        }

        return axis;
}


TraversalNode *TraversalTree::SubdivideNode(TraversalLeaf *leaf,
                                                                                        const AxisAlignedBox3 &box,
                                                                                        AxisAlignedBox3 &backBBox,
                                                                                        AxisAlignedBox3 &frontBBox)
{

        if (TerminationCriteriaMet(leaf))
                return leaf;

        float position;

        // select subdivision axis
        int axis = SelectPlane( leaf, box, position );

        if (axis == -1) {
                cout << "terminate on cost ratio" << endl;
                ++ mStat.costRatioNodes;
                return leaf;
        }

        mStat.nodes+=2;
        mStat.splits[axis]++;

        // add the new nodes to the tree
        TraversalInterior *node = new TraversalInterior(leaf->mParent);

        node->mAxis = axis;
        node->mPosition = position;
        node->mBox = box;

        backBBox = box;
        frontBBox = box;

        // first count ray sides
        int objectsBack = 0;
        int objectsFront = 0;

        backBBox.SetMax(axis, position);
        frontBBox.SetMin(axis, position);

        ViewCellContainer::const_iterator mi, mi_end = leaf->mViewCells.end();

        for (mi = leaf->mViewCells.begin(); mi != mi_end; ++ mi)
        {
                // determine the side of this ray with respect to the plane
                AxisAlignedBox3 box = (*mi)->GetBox();
                if (box.Max(axis) > position) 
                        ++ objectsFront;

                if (box.Min(axis) < position)
                        ++ objectsBack;
        }

        TraversalLeaf *back = new TraversalLeaf(node, objectsBack);
        TraversalLeaf *front = new TraversalLeaf(node, objectsFront);

        back->mDepth = front->mDepth = leaf->mDepth + 1;

        // replace a link from node's parent
        if (leaf->mParent)
        {               
                leaf->mParent->ReplaceChildLink(leaf, node);
        }

        // and setup child links
        node->SetupChildLinks(back, front);

        for (mi = leaf->mViewCells.begin(); mi != mi_end; ++ mi) 
        {
                // determine the side of this ray with respect to the plane
                AxisAlignedBox3 box = (*mi)->GetBox();

                if (box.Max(axis) >= position )
                {
                        front->mViewCells.push_back(*mi);
                }

                if (box.Min(axis) < position )
                {
                        back->mViewCells.push_back(*mi);
                }

                mStat.objectRefs -= (int)leaf->mViewCells.size();
                mStat.objectRefs += objectsBack + objectsFront;
        }

        delete leaf;

        return node;
}


void TraversalTreeStatistics::Print(ostream &app) const
{
        app << "===== TraversalTree statistics ===============\n";

        app << "#N_NODES ( Number of nodes )\n" << nodes << "\n";

        app << "#N_LEAVES ( Number of leaves )\n" << Leaves() << "\n";

        app << "#N_SPLITS ( Number of splits in axes x y z dx dy dz)\n";

        for (int i = 0; i < 7; ++ i)
                app << splits[i] << " ";
        app << endl;

        app << "#N_MAXOBJECTREFS  ( Max number of object refs / leaf )\n" <<
                maxObjectRefs << "\n";

        app << "#N_AVGOBJECTREFS  ( Avg number of object refs / leaf )\n" <<
                totalObjectRefs / (double)Leaves() << "\n";

        app << "#N_LEAFDOMAINREFS  ( Number of query domain Refs / leaf )\n" <<
                objectRefs / (double)Leaves() << "\n";

        app << "#N_PEMPTYLEAVES  ( Percentage of leaves with zero query domains )\n"<<
                zeroQueryNodes * 100 / (double)Leaves() << endl;

        app << "#N_PMAXDEPTHLEAVES ( Percentage of leaves at maxdepth )\n"<<
                maxDepthNodes * 100 / (double)Leaves() << endl;

        app << "#N_PMINCOSTLEAVES  ( Percentage of leaves with minCost )\n"<<
                minCostNodes * 100 / (double)Leaves() << endl;

        app << "#N_COSTRATIOLEAVES ( Percentage of leaves with cost ratio termination )\n"<<
                costRatioNodes * 100 / (double)Leaves() << endl;

        //  app << setprecision(4);
        //  app << "#N_CTIME  ( Construction time [s] )\n"
        //      << Time() << " \n";

        app << "======= END OF TraversalTree statistics ========\n";
}


void TraversalTree::EvaluateLeafStats(const TraversalData &data)
{
        // the node became a leaf -> evaluate stats for leafs
        TraversalLeaf *leaf = (TraversalLeaf *)data.mNode;

        if (data.mDepth >= mTermMaxDepth)
                ++ mStat.maxDepthNodes;

        if ((int)(leaf->mViewCells.size()) <= mTermMinCost)
                ++ mStat.minCostNodes;

        mStat.totalObjectRefs += (int)leaf->mViewCells.size();

        if ((int)(leaf->mViewCells.size()) > mStat.maxObjectRefs)
                mStat.maxObjectRefs = (int)leaf->mViewCells.size();
}


void
TraversalTree::SortSubdivisionCandidates(TraversalLeaf *node,
                                                                                 const int axis)
{
        CLEAR_CONTAINER(*splitCandidates);
        //splitCandidates->clear();

    int requestedSize = 2 * (int)node->mViewCells.size();
        
        // creates a sorted split candidates array
        if (splitCandidates->capacity() > 500000 &&
                requestedSize < (int)(splitCandidates->capacity() / 10) ) 
        {               
                delete splitCandidates;
                splitCandidates = new vector<SortableEntry *>;
        }
  
        splitCandidates->reserve(requestedSize);

        
        // insert all queries 
        for(ViewCellContainer::const_iterator mi = node->mViewCells.begin();
                mi != node->mViewCells.end();
                mi++) 
        {
                AxisAlignedBox3 box = (*mi)->GetBox();

                //cout << "box: " << box << endl;
                splitCandidates->push_back(new SortableEntry(SortableEntry::BOX_MAX,
                        box.Max(axis),
                        *mi)
                        );
        }

        // insert all queries 
        for(ViewCellContainer::const_iterator mi = node->mViewCells.begin();
                mi != node->mViewCells.end();
                mi++) 
        {
                AxisAlignedBox3 box = (*mi)->GetBox();

                splitCandidates->push_back(new SortableEntry(SortableEntry::BOX_MIN,
                        box.Min(axis),
                        *mi)
                        );
                /*splitCandidates->push_back(new SortableEntry(SortableEntry::BOX_MAX,
                        box.Max(axis),
                        *mi)
                        );*/
        }

        stable_sort(splitCandidates->begin(), splitCandidates->end(), iltS);
}


float
TraversalTree::BestCostRatio(TraversalLeaf *node,
                                                         const AxisAlignedBox3 &box,
                                                         const int axis,
                                                         float &position,
                                                         int &objectsBack,
                                                         int &objectsFront
                                                         )
{

#define DEBUG_COST 1

#if DEBUG_COST
        static int nodeId = -1;
        char filename[256];

        static int lastAxis = 100;

        if (axis <= lastAxis)
                ++ nodeId;

        lastAxis = axis;

        sprintf(filename, "sah-cost%d-%d.log", nodeId, axis);
        ofstream costStream;

        if (nodeId < 100)
                costStream.open(filename);

#endif

        SortSubdivisionCandidates(node, axis);

        // go through the lists, count the number of objects left and right
        // and evaluate the following cost funcion:
        // C = ct_div_ci  + (ol + or)/queries

        vector<SortableEntry *>::const_iterator ci;

        int objectsLeft = 0, objectsRight = (int)node->mViewCells.size();

        int dummy1 = objectsLeft, dummy2 = objectsRight;

        float minBox = box.Min(axis);
        float maxBox = box.Max(axis);
        float boxArea = box.SurfaceArea();

        float minBand = minBox + mSplitBorder*(maxBox - minBox);
        float maxBand = minBox + (1.0f - mSplitBorder)*(maxBox - minBox);

        float minSum = 1e20f;

        int openBoxes = 0;

        for(ci = splitCandidates->begin(); ci < splitCandidates->end(); ++ ci) 
        {
                switch ((*ci)->type) 
                {
                case SortableEntry::BOX_MIN:
                        ++ objectsLeft;
                        ++ openBoxes;
                        break;
                case SortableEntry::BOX_MAX:
                        -- objectsRight;
                        -- openBoxes;
                        break;
                }

                if ((*ci)->value >= minBand && (*ci)->value <= maxBand) 
                {
                        AxisAlignedBox3 lbox = box;
                        AxisAlignedBox3 rbox = box;

                        lbox.SetMax(axis, (*ci)->value);
                        rbox.SetMin(axis, (*ci)->value);

                        const float sum = objectsLeft * lbox.SurfaceArea() + objectsRight * rbox.SurfaceArea();

                        // cout<<"pos="<<(*ci).value<<"\t q=("<<ql<<","<<qr<<")\t r=("<<rl<<","<<rr<<")"<<endl;
                        // cout<<"cost= "<<sum<<endl;

#if DEBUG_COST
                        if (nodeId < 100) 
                        {
                                float oldCost = (float)node->mViewCells.size();
                                float newCost = mCt_div_ci + sum / boxArea;
                                float ratio = newCost / oldCost;

                                costStream << (*ci)->value << " " << ratio << " open: " << openBoxes;

                                if ((*ci)->type == SortableEntry::BOX_MAX)
                                        costStream << " max event" << endl;
                                else 
                                        costStream << " min event" << endl;
                        }
#endif

                        if (sum < minSum) 
                        {
                                minSum = sum;
                                position = (*ci)->value;

                                objectsBack = objectsLeft;
                                objectsFront = objectsRight;
                        }
                }
        }

        const float oldCost = (float)node->mViewCells.size();
        const float newCost = mCt_div_ci + minSum / boxArea;
        const float ratio = newCost / oldCost;

        //if (boxArea == 0)
        //      cout << "error: " << boxArea << endl;
        if (ratio > 2)
        {
                cout << "costratio: " << ratio << " oldcost: " << oldCost << " box area: " << boxArea << " new: " << newCost << endl;
                cout << "obj left: " <<objectsBack<< " obj right: " << objectsFront << endl;
                cout << "dummy1: " << dummy1 << " dummy2: " << dummy2 << endl;
        }
#if 0
        cout<<"===================="<<endl;
        cout<<"costRatio="<<ratio<<" pos="<<position<<" t="<<(position - minBox)/(maxBox - minBox)
                        <<"\t o=("<<objectsBack<<","<<objectsFront<<")"<<endl;
#endif

        return ratio;
}


int TraversalTree::FindViewCellIntersections(const Vector3 &lStart, 
                                                                                         const Vector3 &lEnd,
                                                                                         const ViewCellContainer &viewCells,
                                                                                         ViewCellContainer &hitViewCells,
                                                                                         const bool useMailboxing)
{
        int hits = 0;
        ViewCellContainer::const_iterator vit, vit_end = viewCells.end();

        for (vit = viewCells.begin(); vit != vit_end; ++ vit)
        {
                ViewCell *viewCell = *vit;
                // don't have to mail if each view cell belongs to exactly one leaf
                if (!useMailboxing || !viewCell->Mailed())
                {
                        if (useMailboxing)
                viewCell->Mail();

                        // hack: assume that we use vsp tree, 
                        //P so we can just test intersection with bounding boxes
                        if (viewCell->GetBox().Intersects(lStart, lEnd))
                        {
                                hitViewCells.push_back(viewCell);
                                ++ hits;
                        }
                }
        }

        return hits;
}


int TraversalTree::CastLineSegment(const Vector3 &origin,
                                                                   const Vector3 &termination,
                                                                   ViewCellContainer &viewCells,
                                                                   const bool useMailboxing)
{
        int hits = 0;

        float mint = 0.0f, maxt = 1.0f;
        const Vector3 dir = termination - origin;

        stack<LineTraversalData> tStack;

        Vector3 entp = origin;
        Vector3 extp = termination;

        TraversalNode *node = mRoot;
        TraversalNode *farChild;

        float position;
        int axis;

        while (1)
        {
                if (!node->IsLeaf())
                {
                        TraversalInterior *in = static_cast<TraversalInterior *>(node);
                        position = in->mPosition;
                        axis = in->mAxis;

                        if (entp[axis] <= position)
                        {
                                if (extp[axis] <= position)
                                {
                                        node = in->mBack;
                                        // cases N1,N2,N3,P5,Z2,Z3
                                        continue;
                                } else
                                {
                                        // case N4
                                        node = in->mBack;
                                        farChild = in->mFront;
                                }
                        }
                        else
                        {
                                if (position <= extp[axis])
                                {
                                        node = in->mFront;
                                        // cases P1,P2,P3,N5,Z1
                                        continue;
                                }
                                else
                                {
                                        node = in->mFront;
                                        farChild = in->mBack;
                                        // case P4
                                }
                        }

                        // $$ modification 3.5.2004 - hints from Kamil Ghais
                        // case N4 or P4
                        const float tdist = (position - origin[axis]) / dir[axis];
                        tStack.push(LineTraversalData(farChild, extp, maxt)); //TODO

                        extp = origin + dir * tdist;
                        maxt = tdist;
                }
                else
                {
                        // compute intersection with all objects in this leaf
                        TraversalLeaf *leaf = static_cast<TraversalLeaf *>(node);

                        hits += FindViewCellIntersections(origin, 
                                                                                          termination, 
                                                                                          leaf->mViewCells,
                                                                                          viewCells,
                                                                                          useMailboxing);
                        
                        // get the next node from the stack
                        if (tStack.empty())
                                break;

                        entp = extp;
                        mint = maxt;
                        
                        LineTraversalData &s  = tStack.top();
                        node = s.mNode;
                        extp = s.mExitPoint;
                        maxt = s.mMaxT;

                        tStack.pop();
                }
        }

        return hits;
}


void TraversalTree::CollectLeaves(vector<TraversalLeaf *> &leaves)
{
        stack<TraversalNode *> nodeStack;
        nodeStack.push(mRoot);

        while (!nodeStack.empty()) 
        {
                TraversalNode *node = nodeStack.top();
                nodeStack.pop();
                
                if (node->IsLeaf()) 
                {
                        TraversalLeaf *leaf = (TraversalLeaf *)node;
                        leaves.push_back(leaf);
                } 
                else 
                {
                        TraversalInterior *interior = (TraversalInterior *)node;
                        nodeStack.push(interior->mBack);
                        nodeStack.push(interior->mFront);
                }
        }
}


AxisAlignedBox3 TraversalTree::GetBox(const TraversalNode *node) const 
{       
        TraversalInterior *parent = node->mParent;
        
        if (parent == NULL)
                return mBox;

        if (!node->IsLeaf())
                return ((TraversalInterior *)node)->mBox;

        AxisAlignedBox3 box(parent->mBox);
        
        if (parent->mFront == node)
                box.SetMin(parent->mAxis, parent->mPosition);
        else
                box.SetMax(parent->mAxis, parent->mPosition);
        return box;
}

}