source: GTP/trunk/App/Demos/Vis/FriendlyCulling/src/Bvh.cpp @ 3068

#include "Bvh.h"
#include "Camera.h"
#include "Plane3.h"
#include "glInterface.h"
#include "Triangle3.h"
#include "SceneEntity.h"
#include "Geometry.h"
#include "RenderState.h"
#include "Material.h"
#include "gzstream.h"

#include <queue>
#include <stack>
#include <fstream>
#include <iostream>
#include <iomanip>


#ifdef _CRT_SET
        #define _CRTDBG_MAP_ALLOC
        #include <stdlib.h>
        #include <crtdbg.h>

        // redefine new operator
        #define DEBUG_NEW new(_NORMAL_BLOCK, __FILE__, __LINE__)
        #define new DEBUG_NEW
#endif

#define INVALID_TEST ((unsigned int)-1)

using namespace std;

namespace CHCDemoEngine
{

int BvhNode::sCurrentState = 0;


/*
3 x---------x 2
  |\         \
  | \         \
  |  \         \
  |     4 x---------x 5
  |   |         | 
0 x   |     x 1 |
   \  |         |
    \ |         |
     \|         |
        7 x---------x 6             
*/

static unsigned int sIndices[] =        
{7, // degenerated
 7, 6, 4, 5, 3, 2, 0, 1,
 1, 4, // degenerated
 4, 3, 7, 0, 6, 1, 5, 2,
 2 // degenerated
};


const static int sNumIndicesPerBox = 20;

/*      Order of vertices
        0 = (0, 0, 0)
        1 = (1, 0, 0)
        2 = (1, 1, 0)
        3 = (0, 1, 0)
        4 = (0, 1, 1)
        5 = (1, 1, 1)
        6 = (1, 0, 1)
        7 = (0, 0, 1)
*/

static Plane3 sNearPlane;
static float sNear;
static Frustum sFrustum;

/// these values are valid for all nodes
static int sClipPlaneAABBVertexIndices[12];


#define ALIGN_INDICES

BvhNode::BvhNode(BvhNode *parent): 
mParent(parent),
mAxis(-1), 
mDepth(0), 
mFirst(-1), 
mLast(-1),
mNumTestNodes(1),
mTestNodesIdx(-1),
mIndicesPtr(-1),
mId(0),
mIsMaxDepthForVirtualLeaf(false),
mIsVirtualLeaf(false)
{
        for (int i = 0; i < NUM_STATES; ++ i)
        {
                mPlaneMask[i] = 0;
                mPreferredPlane[i]= 0;
                mLastRenderedFrame[i] = -1;
        }
}


BvhNode::~BvhNode() 
{
}


void BvhNode::ResetVisibility()
{
        for (int i = 0; i < NUM_STATES; ++ i)
        {
                mVisibility[i].Reset();
                mLastRenderedFrame[i] = -1;
                mPlaneMask[i] = 0;
                mPreferredPlane[i]= 0;
        }
}


void BvhNode::VisibilityInfo::Reset()
{
        mIsVisible = false;
        mAssumedVisibleFrameId = 0;
        mLastVisitedFrame = -1;
        mTimesInvisible = 0;
        mIsFrustumCulled = false;
        mIsNew = true;
        mLastQueriedFrame = -1;
}


BvhInterior::~BvhInterior() 
{
        DEL_PTR(mBack);
        DEL_PTR(mFront); 
}


BvhLeaf::~BvhLeaf()
{
}


/**********************************************************/
/*                class Bvh implementation                */
/**********************************************************/


Bvh::Bvh()
{
        Init();
}


Bvh::Bvh(const SceneEntityContainer &entities)
{
        Init();

        mGeometrySize = entities.size();
        mGeometry = new SceneEntity*[mGeometrySize];

        SceneEntityContainer::const_iterator it, it_end = entities.end();

        int i = 0;
        for (it = entities.begin(); it != it_end; ++ it, ++ i)
        {
                mGeometry[i] = (*it);
        }
}


Bvh::~Bvh() 
{
        if (mVertices) delete [] mVertices; 
        if (mIndices) delete [] mIndices;
        if (mTestIndices) delete [] mTestIndices;
        if (mGeometry) delete [] mGeometry;

        if (mRoot) delete mRoot;
        // delete vbo
        glDeleteBuffersARB(1, &mVboId);
}


void Bvh::Init()
{
        //mStaticRoot = NULL;
        mDynamicRoot = NULL;
        mRoot = NULL;

        mVertices = NULL;
        mIndices = NULL;
        mTestIndices = NULL;
        mCurrentIndicesPtr = 0;
        mNumNodes = 0;
        
        // nodes are tested using the subnodes from 3 levels below
        mMaxDepthForTestingChildren = 3;
        //mMaxDepthForTestingChildren = 4;

        // the ratio of area between node and subnodes where 
        // testing the subnodes as proxy is still considered feasable
        mAreaRatioThreshold = 2.0f;
        //mAreaRatioThreshold = 1.4f;

        mVboId = -1;

        mMaxDepthForDynamicBranch = 10;
}




//////////////////////
//-- functions that are used during the main traversal

void Bvh::PullUpLastVisited(BvhNode *node, int frameId) const
{               
        BvhNode *parent = node->GetParent();

        while (parent && (parent->GetLastVisitedFrame() != frameId))
        {
                parent->SetLastVisitedFrame(frameId);
                parent = parent->GetParent();
        }
}


void Bvh::MakeParentsVisible(BvhNode *node)
{
        BvhNode *parent = node->GetParent();

        while (parent && (!parent->IsVisible()))
        {
                parent->SetVisible(true);
                parent = parent->GetParent();
        }
}


////////////////////////////////

void Bvh::CollectLeaves(BvhNode *node, BvhLeafContainer &leaves)
{
        stack<BvhNode *> tStack;
        tStack.push(node);

        while (!tStack.empty())
        {
                BvhNode *node = tStack.top();

                tStack.pop();

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

                        tStack.push(interior->mFront);
                        tStack.push(interior->mBack);
                }
                else
                {
                        leaves.push_back(static_cast<BvhLeaf *>(node));
                }
        }
}


void Bvh::CollectNodes(BvhNode *node, BvhNodeContainer &nodes)
{
        stack<BvhNode *> tStack;

        tStack.push(node);

        while (!tStack.empty())
        {
                BvhNode *node = tStack.top();
                tStack.pop();

                nodes.push_back(node);

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

                        tStack.push(interior->mFront);
                        tStack.push(interior->mBack);
                }
        }
}


typedef pair<BvhNode *, int> tPair;

void Bvh::CollectNodes(BvhNode *root, BvhNodeContainer &nodes, int depth)
{
        stack<tPair> tStack;
        tStack.push(tPair(root, 0));

        while (!tStack.empty())
        {
                BvhNode *node = tStack.top().first;
                const int d = tStack.top().second;

                tStack.pop();

                // found node in specified depth => take this node
                if ((d == depth) || (node->IsLeaf()))
                {
                        nodes.push_back(node);
                }
                else
                { 
                        BvhInterior *interior = static_cast<BvhInterior *>(node);

                        tStack.push(tPair(interior->mFront, d + 1));
                        tStack.push(tPair(interior->mBack, d + 1));
                }
        }
}


SceneEntity **Bvh::GetGeometry(BvhNode *node, int &geometrySize) const
{
        geometrySize = node->CountPrimitives();
        return mGeometry + node->mFirst;
}


bool Bvh::TestPlane(BvhNode *node, int i, bool &bIntersect)
{
        // do the test only if necessary
        if (!(node->mPlaneMask[BvhNode::sCurrentState] & (1 << i)))
        {
                return true;
        }


        ////////
        //-- test the n-vertex
                
        if ((node->mBox.GetDistance(sClipPlaneAABBVertexIndices[i * 2 + 0], sFrustum.mClipPlanes[i]) > 0.0f))
        {
                // outside
                node->mPreferredPlane[BvhNode::sCurrentState] = i;
                return false;
        }

        ////////////
        //-- test the p-vertex

        if (node->mBox.GetDistance(sClipPlaneAABBVertexIndices[i * 2 + 1], sFrustum.mClipPlanes[i]) <= 0.0f)
        {
                // completely inside: children don't need to check against this plane no more
                node->mPlaneMask[BvhNode::sCurrentState] ^= 1 << i;
        }
        else 
        {
                bIntersect = true;
        }
        
        return true;
}


int     Bvh::IsWithinViewFrustum(BvhNode *node)
{
        bool bIntersect = false;

        if (node->GetParent())
                node->mPlaneMask[BvhNode::sCurrentState] = node->GetParent()->mPlaneMask[BvhNode::sCurrentState];

        ////////
        //-- apply frustum culling for the planes with index mPreferredPlane to 6

        for (int i = node->mPreferredPlane[BvhNode::sCurrentState]; i < 6; ++ i)
                if (!TestPlane(node, i, bIntersect)) return 0;
        

        //////////
        //-- apply frustum culling for the planes with index 0 to mPreferredPlane

        for (int i = 0; i < node->mPreferredPlane[BvhNode::sCurrentState]; ++ i)
                if (!TestPlane(node, i, bIntersect)) return 0;
        
        return bIntersect ? -1 : 1;
}


static void CalcNPVertexIndices(const Frustum &frustum, int *indices)
{
        for (int i = 0; i < 6; ++ i)
        {
                // n-vertex
                indices[i * 2 + 0] = AxisAlignedBox3::GetIndexNearestVertex(frustum.mClipPlanes[i].mNormal);
                // p-vertex
                indices[i * 2 + 1] = AxisAlignedBox3::GetIndexFarthestVertex(frustum.mClipPlanes[i].mNormal);   
        }
}


void Bvh::InitFrame(Camera *cam)
{
        // = 0011 1111 which means that at the beginning, all six planes have to frustum culled
        mRoot->mPlaneMask[BvhNode::sCurrentState] = 0x3f;

        cam->CalcFrustum(sFrustum);
        CalcNPVertexIndices(sFrustum, sClipPlaneAABBVertexIndices);

        // store near plane
        sNearPlane = Plane3(cam->GetDirection(), cam->GetPosition());
        sNear = cam->GetNear();

        // rebuild dynamic part of the hierarchy
        if (!mDynamicEntities.empty())
        {
                UpdateDynamicBranch();
        }
}


void Bvh::UpdateDistance(BvhNode *node) const
{
        // q: should we use distance to center rather than the distance to the near plane?
        // distance to near plane can also be used for checking near plane intersection
        //node->mDistance = sNearPlane.Distance(node->GetBox().Center());
        node->mDistance = node->GetBox().GetMinDistance(sNearPlane);
}


float Bvh::CalcMaxDistance(BvhNode *node) const
{
#if 1
        return node->GetBox().GetMaxDistance(sNearPlane);

#else
        // use bounding boxes of geometry to determine max dist
        float maxDist = .0f;

        int geometrySize;
        SceneEntity **entities = GetGeometry(node, geometrySize);

        for (int i = 0; i < geometrySize; ++ i)
        {
                SceneEntity *ent = entities[i];
                float dist = ent->GetTransformedBoundingBox().GetMaxDistance(sNearPlane);

                if (dist > maxDist) maxDist = dist;
        }

        return maxDist;
#endif
}


void Bvh::RenderBounds(BvhNode *node, RenderState *state, bool useTightBounds)
{
        // hack: use dummy contayiner as wrapper in order to use multibox function
        static BvhNodeContainer dummy(1);
        dummy[0] = node;
        RenderBounds(dummy, state, useTightBounds);
}


int Bvh::RenderBounds(const BvhNodeContainer &nodes, 
                                          RenderState *state, 
                                          bool useTightBounds)
{
        int renderedBoxes;

        if (!useTightBounds)
        {
                // if not using tight bounds, rendering boxes in immediate mode
                // is preferable to vertex arrays (less setup time)
                BvhNodeContainer::const_iterator nit, nit_end = nodes.end();
                        
                for (nit = nodes.begin(); nit != nit_end; ++ nit)
                {
                        RenderBoundingBoxImmediate((*nit)->GetBox());
                }

                renderedBoxes = (int)nodes.size();
        }
        else
        {
                renderedBoxes = PrepareBoundsWithDrawArrays(nodes);
                RenderBoundsWithDrawArrays(renderedBoxes, state);
        }

        return renderedBoxes;
}


int Bvh::PrepareBoundsWithDrawArrays(const BvhNodeContainer &nodes)
{
        ///////////////////
        //-- for the first time we come here => create vbo and indices

        if (!mIndices)
        {       
                // create list of indices
                CreateIndices();
        }

        if (mVboId == -1)
        {       
                // prepare the vbo 
                PrepareVertices();
        }

        ///////////////

        int numNodes = 0;

        BvhNodeContainer::const_iterator nit, nit_end = nodes.end();

        for (nit = nodes.begin(); nit != nit_end; ++ nit)
        {
                BvhNode *node = *nit;
        const int numIndices = node->mNumTestNodes * sNumIndicesPerBox;
                
                // copy indices
                memcpy(mIndices + numNodes * sNumIndicesPerBox, 
                           mTestIndices + node->mIndicesPtr, 
                           numIndices * sizeof(unsigned int));

                numNodes += node->mNumTestNodes;
        }

        return numNodes;
}


void Bvh::RenderBoundsWithDrawArrays(int numNodes, RenderState *state)
{
        /////////
        //-- Render the vbo

        if (state->GetCurrentVboId() != mVboId)
        {
                glBindBufferARB(GL_ARRAY_BUFFER_ARB, mVboId);
                // set the vertex pointer to the vertex buffer
                glVertexPointer(3, GL_FLOAT, 0, (char *)NULL);  

                state->SetCurrentVboId(mVboId);
        }

        // we do use the last or the first index (they are generate and only used to connect strips)
        int numElements = numNodes * sNumIndicesPerBox - 1;
        // don't render first degenerate index
        glDrawElements(GL_TRIANGLE_STRIP, numElements, GL_UNSIGNED_INT, mIndices + 1); 
}


void Bvh::CreateIndices()
{
        // collect bvh nodes
        BvhNodeContainer nodes;
        CollectNodes(mRoot, nodes);

        cout << "creating new indices" << endl;

        int numMaxIndices = 0;

        BvhNodeContainer::const_iterator lit, lit_end = nodes.end();

        for (lit = nodes.begin(); lit != lit_end; ++ lit)
        {
                int offset = (*lit)->mNumTestNodes * sNumIndicesPerBox;
#ifdef ALIGN_INDICES
                // align with 32 in order to speed up memcopy
                offset = (offset / 32) * 32 + 32; 
#endif
                numMaxIndices += offset;
        }

        cout << "creating global indices buffer" << endl;

        if (mIndices) delete [] mIndices;
        if (mTestIndices) delete [] mTestIndices;

        // global buffer: create it once so we don't have
        // to allocate memory from the chunks of the node
        mIndices = new unsigned int[numMaxIndices];
        // create new index buffer for the individual nodes
        mTestIndices = new unsigned int[numMaxIndices];
        
        mCurrentIndicesPtr = 0;

        for (lit = nodes.begin(); lit != lit_end; ++ lit)
        {
                BvhNode *node = *lit;
                
                // resize array
                node->mIndicesPtr = mCurrentIndicesPtr;
                int numIndices = 0;

                // the bounding boxes of the test nodes are rendered instead of the node itself
                // => store their indices
                for (int i = 0; i < node->mNumTestNodes; ++ i, numIndices += sNumIndicesPerBox)
                {
                        BvhNode *testNode = mTestNodes[node->mTestNodesIdx + i];

                        // add indices to root node
                        for (int j = 0; j < sNumIndicesPerBox; ++ j)
                        {
                                mTestIndices[mCurrentIndicesPtr + numIndices + j] = sIndices[j] + testNode->GetId() * 8;
                        }
                }

                // align with 32
#ifdef ALIGN_INDICES
                const int offset = (numIndices / 32) * 32 + 32;
#else
                const int offset = numIndices;
#endif
                mCurrentIndicesPtr += offset;
        }
}


void Bvh::ComputeIds()
{
        // collect all nodes
        BvhNodeContainer nodes;
        CollectNodes(mRoot, nodes);

        // assign unique ids to all nodes of the hierarchy
        int i = 0;
        BvhNodeContainer::const_iterator lit, lit_end = nodes.end();

        for (lit = nodes.begin(); lit != lit_end; ++ lit, ++ i)
        {
                (*lit)->SetId(i);
        }
}


void Bvh::PrepareVertices()
{
        // collect all nodes
        BvhNodeContainer nodes;

        nodes.reserve(GetNumStaticNodes());
        CollectNodes(mRoot, nodes);

        const unsigned int bufferSize = 8 * (int)nodes.size();
        mVertices = new Vector3[bufferSize];
        
        int i = 0;
        
        // store bounding box vertices
        BvhNodeContainer::const_iterator lit, lit_end = nodes.end();

        for (lit = nodes.begin(); lit != lit_end; ++ lit, i += 8)
        {
                BvhNode *node = *lit;

                for (int j = 0; j < 8; ++ j)
                        (static_cast<Vector3 *>(mVertices))[node->GetId() * 8 + j] = node->GetBox().GetVertex(j);
        }

        glGenBuffersARB(1, &mVboId);
        glBindBufferARB(GL_ARRAY_BUFFER_ARB, mVboId);
        glVertexPointer(3, GL_FLOAT, 0, (char *)NULL);
        
        glBufferDataARB(GL_ARRAY_BUFFER_ARB, 
                            bufferSize * sizeof(Vector3), 
                            mVertices, 
                                        GL_STATIC_DRAW_ARB);

        glBindBufferARB(GL_ARRAY_BUFFER_ARB, 0);

        // data handled by graphics driver from now on
        DEL_PTR(mVertices);

        cout << "******** created vbos for tighter bounds *********" << endl;
}


void Bvh::SetMaxDepthForTestingChildren(int maxDepth) 
{
        if (maxDepth != mMaxDepthForTestingChildren)
        {
                mMaxDepthForTestingChildren = maxDepth;
                RecomputeBounds();
        }
}


void Bvh::SetAreaRatioThresholdForTestingChildren(float ratio) 
{
        if (ratio != mAreaRatioThreshold)
        {
                mAreaRatioThreshold = ratio;
                RecomputeBounds();
        }
}


void Bvh::RecomputeBounds()
{
        // collect all nodes
        BvhNodeContainer nodes;
        CollectNodes(mRoot, nodes);

        cout << "recomputing bounds, children will be tested in depth " << mMaxDepthForTestingChildren << endl;

        int success = 0;
        BvhNodeContainer::const_iterator lit, lit_end = nodes.end();

        for (lit = nodes.begin(); lit != lit_end; ++ lit)
        {
                BvhNode *node = *lit;

                // recreate list of nodes that will be queried as a proxy
                if (CreateNodeRenderList(node))
                        ++ success;
        }

        float p = 100.0f * (float)success / nodes.size();
        cout << "created tighter bounds for " << p << " percent of the nodes" << endl;

        // recreate indices used for indirect mode rendering
        if (mIndices) CreateIndices();
}

        
bool Bvh::CreateNodeRenderList(BvhNode *node)
{
        BvhNodeContainer children;

        // collect nodes that will be tested instead of the leaf node
        // in order to get a tighter bounding box test
        CollectNodes(node, children, mMaxDepthForTestingChildren);
        

        // using the tighter bounds is not feasable in case
        // that the tighter bounds represent nearly the same projected area
        // as the old bounding box. Test this property using either 
        // volume or area heuristics

        float vol = 0;
        float area = 0;

        BvhNodeContainer::const_iterator cit;

        for (cit = children.begin(); cit != children.end(); ++ cit)
                area += (*cit)->GetBox().SurfaceArea();

        const float areaRatio = area / node->GetBox().SurfaceArea();
        
        bool success;

        if (areaRatio < mAreaRatioThreshold)
                success = true;
        else
        {
                // hack: only store node itself
                children.clear();
                children.push_back(node);

                success = false;
        }

        // the new test nodes are added at the end of the vector
        node->mTestNodesIdx = (int)mTestNodes.size();

        // use the found nodes as nodes during the occlusion tests
        for (cit = children.begin(); cit != children.end(); ++ cit)
        {
                BvhNode *child = *cit;
                mTestNodes.push_back(child);
        }

        node->mNumTestNodes = (int)children.size();

        return success;
}


void Bvh::ResetNodeClassifications()
{
        BvhNodeContainer nodes;

        nodes.reserve(GetNumNodes());
        CollectNodes(mRoot, nodes);

        BvhNodeContainer::const_iterator lit, lit_end = nodes.end();

        for (lit = nodes.begin(); lit != lit_end; ++ lit)
        {
                (*lit)->ResetVisibility();
        }
}


void Bvh::ComputeBvhStats() 
{
        std::stack<BvhNode *> nodeStack;
        nodeStack.push(mRoot);

        int numVirtualLeaves = 0;

        while (!nodeStack.empty()) 
        {
                BvhNode *node = nodeStack.top();
                nodeStack.pop();

                if (node->IsVirtualLeaf()) 
                {
                        ++ numVirtualLeaves;

                        BvhLeaf *leaf = static_cast<BvhLeaf *>(node);

                        mBvhStats.mTriangles += CountTriangles(leaf);
                        mBvhStats.mLeafSA += leaf->mBox.SurfaceArea();
                        mBvhStats.mLeafVol += leaf->mBox.GetVolume();
                } 
                else 
                {
                        mBvhStats.mInteriorSA += node->mBox.SurfaceArea();
                        mBvhStats.mInteriorVol += node->mBox.GetVolume();

                        BvhInterior *interior = static_cast<BvhInterior *>(node);
                        
                        nodeStack.push(interior->mBack);
                        nodeStack.push(interior->mFront);
                }
        }

        mBvhStats.mGeometryRatio = mGeometrySize / (float)numVirtualLeaves;
        mBvhStats.mTriangleRatio = mBvhStats.mTriangles / (float)numVirtualLeaves;
}


void Bvh::PrintBvhStats() const
{
        cout << "\n******** bvh stats: ***********" << endl;
        cout << "interiorNodesSA = " << mBvhStats.mInteriorSA / mRoot->mBox.SurfaceArea() << endl;
        cout << "leafNodesSA = " << mBvhStats.mLeafSA / mRoot->mBox.SurfaceArea() << endl;
        cout << "interiorNodesVolume = " << mBvhStats.mInteriorVol / mRoot->mBox.GetVolume() << endl;
        cout << "leafNodesVolume = " << mBvhStats.mLeafVol / mRoot->mBox.GetVolume() << endl;

        cout << "geometry per leaf: " <<  mBvhStats.mGeometryRatio << endl;
        cout << "triangles per leaf: " <<  mBvhStats.mTriangleRatio << endl;
        cout << "**************" << endl << endl;
}


int Bvh::CountTriangles(BvhNode *node) const
{
        int numTriangles = 0;

        for (int i = node->mFirst; i <= node->mLast; ++ i)
        {
                numTriangles += mGeometry[i]->CountNumTriangles(0);
        }

        return numTriangles;
}


float Bvh::GetArea(BvhNode *node) const
{
        return node->mArea;
}


void Bvh::UpdateNumLeaves(BvhNode *node) const
{
        if (node->IsLeaf())
        {
                node->mNumLeaves = 1;
        }
        else
        {
                BvhNode *f = static_cast<BvhInterior *>(node)->GetFront();
                BvhNode *b = static_cast<BvhInterior *>(node)->GetBack();

                UpdateNumLeaves(f);
                UpdateNumLeaves(b);

                node->mNumLeaves = f->mNumLeaves + b->mNumLeaves;
        }
}


void Bvh::CollectVirtualLeaves(BvhNode *node, BvhNodeContainer &leaves)
{
        stack<BvhNode *> tStack;
        tStack.push(node);

        while (!tStack.empty())
        {
                BvhNode *node = tStack.top();
                tStack.pop();

                if (node->mIsVirtualLeaf)
                {
                        leaves.push_back(node);
                }
                else if (!node->IsLeaf())
                {
                        BvhInterior *interior = static_cast<BvhInterior *>(node);

                        tStack.push(interior->mFront);
                        tStack.push(interior->mBack);
                }
        }
}


void Bvh::SetVirtualLeaves(int numTriangles) 
{
        // first invalidate old virtual leaves
        BvhNodeContainer leaves;
        CollectVirtualLeaves(mRoot, leaves);

        BvhNodeContainer::const_iterator bit, bit_end = leaves.end();

        for (bit = leaves.begin(); bit != bit_end; ++ bit)
        {
                (*bit)->mIsVirtualLeaf = false;
        }

        mNumVirtualNodes = 0;
        // assign new virtual leaves based on specified #triangles per leaf
        std::stack<BvhNode *> nodeStack;
        nodeStack.push(mRoot);

        while (!nodeStack.empty()) 
        {
                BvhNode *node = nodeStack.top();
                nodeStack.pop();

                ++ mNumVirtualNodes;

                if (node->IsLeaf()) 
                {
                        BvhLeaf *leaf = static_cast<BvhLeaf *>(node);
                        leaf->mIsVirtualLeaf = true;
                } 
                else 
                {
                        BvhInterior *interior = static_cast<BvhInterior *>(node);

                        BvhNode *f = interior->mFront;
                        BvhNode *b = interior->mBack;

                        if (node->mIsMaxDepthForVirtualLeaf || 
                                (CountTriangles(node) <= numTriangles))
                        {
                                 node->mIsVirtualLeaf = true;
                        }
                        else
                        {
                                nodeStack.push(interior->mBack);
                                nodeStack.push(interior->mFront);
                        }
                }
        }

        /// Reset the node states
        ResetNodeClassifications();
}


void Bvh::PostProcess() 
{
        // this function must be called once after hierarchy creation

        // We initialize the virtual leaves
        // of this bvh, i.e., the nodes that are used as
        // leaves of the hierarchy during traversal.

        // Initially they are set either
        // a) to the real leaves of the hierarchy or
        // b) the point where the subdivision on object level ends
        // and the subsequent nodes are just used to provide tighter bounds
        // (see article for the notations)

        std::stack<BvhNode *> nodeStack;
        nodeStack.push(mRoot);

        while (!nodeStack.empty()) 
        {
                BvhNode *node = nodeStack.top();
                nodeStack.pop();

                if (node->IsLeaf()) 
                {
                        node->mIsMaxDepthForVirtualLeaf = true;
                } 
                else 
                {
                        BvhInterior *interior = static_cast<BvhInterior *>(node);

                        BvhNode *f = interior->mFront;
                        BvhNode *b = interior->mBack;

                        if ((f->mFirst == b->mFirst) && (f->mLast == b->mLast))
                        {
                                // point reached where beyond there would be no further reduction 
                                // as both subtrees contain the same objects => stop here
                                // The tree beyond the current node is used to describe
                                // tighter bounds on the geometry contained  in it
                                node->mIsMaxDepthForVirtualLeaf = true;
                        }
                        else
                        {
                                nodeStack.push(f);
                                nodeStack.push(b);
                        }
                }
        }
}


void Bvh::RenderBoundingBoxImmediate(const AxisAlignedBox3 &box)
{
        const Vector3 l = box.Min();
        const Vector3 u = box.Max();

        ///////////
        //-- render AABB as triangle strips

        glBegin(GL_TRIANGLE_STRIP);

        // render first half of AABB
        glVertex3f(l.x, l.y, u.z);
        glVertex3f(u.x, l.y, u.z);
        glVertex3f(l.x, u.y, u.z);
        glVertex3f(u.x, u.y, u.z);
        glVertex3f(l.x, u.y, l.z); 
        glVertex3f(u.x, u.y, l.z);
        glVertex3f(l.x, l.y, l.z); 
        glVertex3f(u.x, l.y, l.z);

        glPrimitiveRestartNV();

        // render second half of AABB
        glVertex3f(l.x, u.y, u.z); 
        glVertex3f(l.x, u.y, l.z);
        glVertex3f(l.x, l.y, u.z);
        glVertex3f(l.x, l.y, l.z);
        glVertex3f(u.x, l.y, u.z); 
        glVertex3f(u.x, l.y, l.z);
        glVertex3f(u.x, u.y, u.z); 
        glVertex3f(u.x, u.y, l.z);

        glEnd();
}


static void RenderBoxForViz(const AxisAlignedBox3 &box)
{
        glBegin(GL_LINE_LOOP);
        glVertex3d(box.Min().x, box.Max().y, box.Min().z);
        glVertex3d(box.Max().x, box.Max().y, box.Min().z);
        glVertex3d(box.Max().x, box.Min().y, box.Min().z);
        glVertex3d(box.Min().x, box.Min().y, box.Min().z);
        glEnd();

        glBegin(GL_LINE_LOOP);
        glVertex3d(box.Min().x, box.Min().y, box.Max().z);
        glVertex3d(box.Max().x, box.Min().y, box.Max().z);
        glVertex3d(box.Max().x, box.Max().y, box.Max().z);
        glVertex3d(box.Min().x, box.Max().y, box.Max().z);
        glEnd();

        glBegin(GL_LINE_LOOP);
        glVertex3d(box.Max().x, box.Min().y, box.Min().z);
        glVertex3d(box.Max().x, box.Min().y, box.Max().z);
        glVertex3d(box.Max().x, box.Max().y, box.Max().z);
        glVertex3d(box.Max().x, box.Max().y, box.Min().z);
        glEnd();

        glBegin(GL_LINE_LOOP);
        glVertex3d(box.Min().x, box.Min().y, box.Min().z);
        glVertex3d(box.Min().x, box.Min().y, box.Max().z);
        glVertex3d(box.Min().x, box.Max().y, box.Max().z);
        glVertex3d(box.Min().x, box.Max().y, box.Min().z);
        glEnd();

        glBegin(GL_LINE_LOOP);
        glVertex3d(box.Min().x, box.Min().y, box.Min().z);
        glVertex3d(box.Max().x, box.Min().y, box.Min().z);
        glVertex3d(box.Max().x, box.Min().y, box.Max().z);
        glVertex3d(box.Min().x, box.Min().y, box.Max().z);
        glEnd();

        glBegin(GL_LINE_LOOP);
        glVertex3d(box.Min().x, box.Max().y, box.Min().z);
        glVertex3d(box.Max().x, box.Max().y, box.Min().z);
        glVertex3d(box.Max().x, box.Max().y, box.Max().z);
        glVertex3d(box.Min().x, box.Max().y, box.Max().z);

        glEnd();
}


static Technique GetVizTechnique()
{
        Technique tech;
        tech.Init();

        //tech.SetLightingEnabled(false);
        //tech.SetDepthWriteEnabled(false);

        tech.SetEmmisive(RgbaColor(1.0f, 1.0f, 1.0f, 1.0f));
        tech.SetDiffuse(RgbaColor(1.0f, 1.0f, 1.0f, 1.0f));
        tech.SetAmbient(RgbaColor(1.0f, 1.0f, 1.0f, 1.0f));

        return tech;
}


void Bvh::RenderBoundsForViz(BvhNode *node, 
                                                         RenderState *state, 
                                                         bool useTightBounds)
{
        Technique *oldTech = state->GetState();
        // we set a simple material
        static Technique boxMat = GetVizTechnique();
        boxMat.Render(state);

        if (!useTightBounds)
        {
                RenderBoxForViz(node->GetBox());
                /*glPolygonMode(GL_FRONT, GL_LINE);
                RenderBoundingBoxImmediate(node->GetBox());
                glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);*/
        }
        else
        {
                for (int i = 0; i < node->mNumTestNodes; ++ i)
                {
                        RenderBoxForViz(mTestNodes[node->mTestNodesIdx + i]->GetBox());
                }
        }

        if (oldTech) oldTech->Render(state);
}



////////////////////////
//-- functions for construction of the dynamic hierarchy

int Bvh::SortTriangles(BvhLeaf *leaf, 
                                           int axis, 
                                           float position,
                                           SceneEntityContainer &entities)
{
        int i = leaf->mFirst;
        int j = leaf->mLast;

    while (1) 
        {
                while (entities[i]->GetCenter()[axis] < position) 
                        ++ i;
        
                while (position < entities[j]->GetCenter()[axis])
                        -- j;

                // sorting finished
                if (i >= j) break;

                // swap entities
                swap(entities[i], entities[j]);
                        
                ++ i;
                -- j;
        }

        return j;
}


int Bvh::SortTrianglesSpatialMedian(BvhLeaf *leaf, 
                                                                        int axis,
                                                                        SceneEntityContainer &entities)
{
        // spatial median
        float m = leaf->mBox.Center()[axis];
        return SortTriangles(leaf, axis, m, entities);
}


BvhNode *Bvh::SubdivideLeaf(BvhLeaf *leaf, 
                                                        int parentAxis, 
                                                        SceneEntityContainer &entities)
{
        if (TerminationCriteriaMet(leaf))
        {
                leaf->mIsVirtualLeaf = true;
                return leaf;
        }

        //int axis = leaf->mBox.MajorAxis();
        int axis = (parentAxis + 1) % 3;

        // position of the split in the partailly sorted array of triangles
        // corresponding to this leaf
        int split = -1;
        const int scale = 20;

        float pos = -1.0f;

        // Spatial median subdivision
        split = SortTrianglesSpatialMedian(leaf, axis, entities);
        pos = leaf->mBox.Center()[axis];
        
        if (split == leaf->mLast)
        {
                // no split could be achieved => just halve number of objects
                split = (leaf->mLast - leaf->mFirst) / 2;
                cerr << "no reduction " << leaf->CountPrimitives() << " " << leaf->mFirst << " " << leaf->mLast << endl;
        }

        // create two more nodes
        mNumNodes += 2;
        BvhInterior *parent = new BvhInterior(leaf->GetParent());
        BvhLeaf *front = new BvhLeaf(parent);
        
        parent->mAxis = axis;
        parent->mBox = leaf->mBox;
        parent->mDepth = leaf->mDepth;

        parent->mBack = leaf;
        parent->mFront = front;
        //parent->mPosition = pos;
        
        // now assign the triangles to the subnodes
        front->mFirst = split + 1;
        front->mLast = leaf->mLast;
        front->mDepth = leaf->mDepth + 1;

        leaf->mLast = split;
        leaf->mDepth = front->mDepth;
        leaf->mParent = parent;
        
        // reset box
        leaf->mBox.Initialize();

        // recursively continue subdivision
        parent->mBack = SubdivideLeaf(static_cast<BvhLeaf *>(parent->mBack), axis, entities);
        parent->mFront = SubdivideLeaf(static_cast<BvhLeaf *>(parent->mFront), axis, entities);
        
        return parent;
}


bool Bvh::TerminationCriteriaMet(BvhLeaf *leaf) const
{
        const bool criteriaMet = 
                (leaf->mDepth > mMaxDepthForDynamicBranch) ||
                (leaf->CountPrimitives() == 1);

        return criteriaMet;
}


void Bvh::UpdateDynamicBranch()
{
        // delete old branch
        if (!mDynamicRoot->IsLeaf())
        {
                cout << "deleting old branch" << endl;

                DEL_PTR(mDynamicRoot);

                mDynamicRoot = new BvhLeaf(mRoot);
                mDynamicRoot->mBox = mRoot->mBox;

                mDynamicRoot->mFirst = 0;
                mDynamicRoot->mLast = 0;
                mDynamicRoot->mArea = mDynamicRoot->mBox.SurfaceArea();
        }

        cout << "updating dynamic branch" << endl;

        mDynamicRoot = SubdivideLeaf(static_cast<BvhLeaf *>(mDynamicRoot), 0, mDynamicEntities);

        cout << "finished updating dynamic branch" << endl;
}


void Bvh::AddDynamicObject(SceneEntity *ent)
{
        mDynamicEntities.push_back(ent);
}


bool Bvh::IntersectsNearPlane(BvhNode *node) const
{ 
        // note: we have problems with large scale object penetrating the near plane 
        // (e.g., objects in the distance which are always handled to be visible)
        // especially annoying is this problem when using the frustum 
        // fitting on the visible objects for shadow mapping
        // but don't see how to solve this issue without using much costlier calculations
        
        // we stored the near plane distance => we can use it also here
        float distanceToNearPlane = node->GetDistance();
        //float distanceToNearPlane = node->GetBox().GetMinDistance(sNearPlane);
        
        return (distanceToNearPlane < sNear);
}


}