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

#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 NUM_INDICES_PER_BOX = 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                */
/**********************************************************/



inline AxisAlignedBox3 ComputeBoundingBox(SceneEntity **entities, int numEntities)
{
        AxisAlignedBox3 box;
        
        if (!numEntities)
        {       // no box=> just initialize
                box.Initialize();
        }
        else
        {
                box = entities[0]->GetWorldBoundingBox();

                for (int i = 1; i < numEntities; ++ i)
                {
                        box.Include(entities[i]->GetWorldBoundingBox());
                }
        }

        return box;
}


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


Bvh::Bvh(const SceneEntityContainer &staticEntities, 
                 const SceneEntityContainer &dynamicEntities)
{
        Init();

        mGeometrySize = staticEntities.size() + dynamicEntities.size();
        mGeometry = new SceneEntity*[mGeometrySize];

        mStaticGeometrySize = staticEntities.size();
        mDynamicGeometrySize = dynamicEntities.size();

        for (size_t i = 0; i < mStaticGeometrySize; ++ i)
        {
                mGeometry[i] = staticEntities[i];
        }

        for (size_t i = 0; i < mDynamicGeometrySize; ++ i)
        {
                mGeometry[mStaticGeometrySize + i] = dynamicEntities[i];
        }

        cout << "max depth for testing children" << mMaxDepthForTestingChildren << endl;
}


Bvh::Bvh(const SceneEntityContainer &staticEntities, 
                 const SceneEntityContainer &dynamicEntities,
                 int maxDepthForTestingChildren)
{
        Init();

        mGeometrySize = staticEntities.size() + dynamicEntities.size();
        mGeometry = new SceneEntity*[mGeometrySize];

        mStaticGeometrySize = staticEntities.size();
        mDynamicGeometrySize = dynamicEntities.size();

        for (size_t i = 0; i < mStaticGeometrySize; ++ i)
        {
                mGeometry[i] = staticEntities[i];
        }

        for (size_t i = 0; i < mDynamicGeometrySize; ++ i)
        {
                mGeometry[mStaticGeometrySize + i] = dynamicEntities[i];
        }

        mMaxDepthForTestingChildren = maxDepthForTestingChildren;
        cout << "max depth for testing children" << mMaxDepthForTestingChildren << endl;
}


Bvh::~Bvh() 
{
        DEL_ARRAY_PTR(mVertices);
        DEL_ARRAY_PTR(mIndices);
        DEL_ARRAY_PTR(mTestIndices);
        DEL_ARRAY_PTR(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, RenderState *state)
{
        // = 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();

        // update dynamic part of the hierarchy
        //if (!mDynamicEntities.empty())
        if (mDynamicGeometrySize)
        {
                UpdateDynamicBranch(mDynamicRoot);
                UpdateDynamicBounds(state);
        }
}


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->GetWorldBoundingBox().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 * NUM_INDICES_PER_BOX;
                
                // copy indices
                memcpy(mIndices + numNodes * NUM_INDICES_PER_BOX, 
                           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 don't use the last or the first index (they are generate and only used to connect strips)
        int numElements = numNodes * NUM_INDICES_PER_BOX - 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;
        // first collect dynamic nodes so we make sure that they are in the beginning
        CollectNodes(mDynamicRoot, nodes);
        // then collect static nodes
        CollectNodes(mStaticRoot, 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 * NUM_INDICES_PER_BOX;
#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 for each individual chunk 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 += NUM_INDICES_PER_BOX)
                {
                        BvhNode *testNode = mTestNodes[node->mTestNodesIdx + i];

                        // add vertex indices of boxes to root node
                        for (int j = 0; j < NUM_INDICES_PER_BOX; ++ 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);
        // first collect dynamic nodes so we make sure that they are in the beginning
        CollectNodes(mDynamicRoot, nodes);
        // then collect static nodes
        CollectNodes(mStaticRoot, nodes);
        // also add root
        nodes.push_back(mRoot);

        // assign 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(GetNumNodes());
        // first collect dynamic nodes so we make sure that they are in the beginning
        CollectNodes(mDynamicRoot, nodes);
        // then collect static nodes
        CollectNodes(mStaticRoot, nodes);
        // also add root
        nodes.push_back(mRoot);

        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;

                Vector3 v;

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

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

        glBindBufferARB(GL_ARRAY_BUFFER_ARB, 0);

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

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


void Bvh::UpdateDynamicBounds(RenderState *state)
{
        // vbos not created yet
        if (mVboId == -1) return;

        // collect all nodes
        static BvhNodeContainer nodes;
        nodes.clear();
        //nodes.reserve(GetNumNodes());
        CollectNodes(mDynamicRoot, nodes);

        const unsigned int bufferSize = 8 * (int)nodes.size();
        if (!mVertices) 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);
        }
        
        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);
        }

        glBufferSubDataARB(GL_ARRAY_BUFFER_ARB, 0,
                                           bufferSize * sizeof(Vector3), 
                                           mVertices);
}


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 collected nodes as proxy for 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(BvhNode *root, BvhStats &bvhStats) 
{
        bvhStats.Reset();
        std::stack<BvhNode *> nodeStack;
        nodeStack.push(root);

        int numVirtualLeaves = 0;
        int numGeometry = 0;

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

                if (node->IsVirtualLeaf()) 
                {
                        ++ numVirtualLeaves;
                        numGeometry += node->CountPrimitives();

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

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

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

        bvhStats.mGeometryRatio = (float)numGeometry / numVirtualLeaves;
        bvhStats.mTriangleRatio = (float)bvhStats.mTriangles / numVirtualLeaves;
        bvhStats.mLeaves = numVirtualLeaves;
}


void Bvh::PrintBvhStats(const BvhStats &bvhStats) const
{
        cout << "\n============ BVH statistics =============" << endl;
        cout << "interiorNodesSA = " << bvhStats.mInteriorSA / mRoot->mBox.SurfaceArea() << endl;
        cout << "leafNodesSA = " << bvhStats.mLeafSA / mRoot->mBox.SurfaceArea() << endl;
        cout << "interiorNodesVolume = " << bvhStats.mInteriorVol / mRoot->mBox.GetVolume() << endl;
        cout << "leafNodesVolume = " << bvhStats.mLeafVol / mRoot->mBox.GetVolume() << endl;

        cout << "geometry per leaf: " << bvhStats.mGeometryRatio << endl;
        cout << "triangles per leaf: " << bvhStats.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::ComputeMaxDepthForVirtualLeaves()
{
        // We initialize the maximal depth for 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);
                        }
                }
        }
}


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

        ComputeMaxDepthForVirtualLeaves();
        // set virtual leaves for specified number of triangles
        SetVirtualLeaves(INITIAL_TRIANGLES_PER_VIRTUAL_LEAVES);
        /// for each node compute the number of leaves under this node
        UpdateNumLeaves(mRoot);
        // compute new ids
        ComputeIds();
        // specify bounds for occlusion tests
        RecomputeBounds();

        mBox = mRoot->GetBox();

        // compute and print stats
        ComputeBvhStats(mRoot, mBvhStats);
        PrintBvhStats(mBvhStats);

        if (mDynamicGeometrySize)
        {
                BvhStats bvhStats;
                ComputeBvhStats(mDynamicRoot, bvhStats);

                cout << "\n=========== Dynamic BVH statistics: =========" << endl;
                cout << "leaves = " << bvhStats.mLeaves << endl;
                cout << "interiorNodesSA = " << bvhStats.mInteriorSA << endl;
                cout << "leafNodesSA = " << bvhStats.mLeafSA << endl;
                cout << "interiorNodesVolume = " << bvhStats.mInteriorVol  << endl;
                cout << "leafNodesVolume = " << bvhStats.mLeafVol << endl;

                cout << "geometry per leaf: " << bvhStats.mGeometryRatio << endl;
                cout << "triangles per leaf: " << bvhStats.mTriangleRatio << endl;
                cout << "=============================================" << endl << endl;
        }
}


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
                                           )
{
        int i = leaf->mFirst;
        int j = leaf->mLast;

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

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

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

        return j;
}


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


BvhNode *Bvh::SubdivideLeaf(BvhLeaf *leaf, 
                                                        int parentAxis
                                                        )
{
        if (TerminationCriteriaMet(leaf))
        {
                //cout << "leaf constructed:" << leaf->mBox << " " << leaf->mFirst << " " << leaf->mLast << endl;
                return leaf;
        }

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

        const int scale = 20;

        // position of the split in the partailly sorted array of triangles
        // corresponding to this leaf
        int split = -1;
        float pos = -1.0f;
        
        // Spatial median subdivision
        split = SortTrianglesSpatialMedian(leaf, axis);
        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());
        parent->mFirst = leaf->mFirst;
        parent->mLast = leaf->mLast;
        
        parent->mAxis = axis;
        parent->mBox = leaf->mBox;
        parent->mDepth = leaf->mDepth;

        BvhLeaf *front = new BvhLeaf(parent);

        parent->mBack = leaf;
        parent->mFront = front;
                
        // 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;
        
        front->mBox = ComputeBoundingBox(mGeometry + front->mFirst, front->CountPrimitives());
        leaf->mBox = ComputeBoundingBox(mGeometry + leaf->mFirst, leaf->CountPrimitives());

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


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

        return criteriaMet;
}


void Bvh::UpdateDynamicBranch(BvhNode *node)
{
        if (node->IsLeaf())
        {
                int numEntities;
                SceneEntity **entities = GetGeometry(node, numEntities);

                node->mBox = ComputeBoundingBox(entities, numEntities);
                //cout << "box: " << node->mBox << endl;
        }
        else
        {
                BvhNode *f = static_cast<BvhInterior *>(node)->GetFront();
                BvhNode *b = static_cast<BvhInterior *>(node)->GetBack();

                UpdateDynamicBranch(f);
                UpdateDynamicBranch(b);

                node->mBox = f->mBox;
                node->mBox.Include(b->mBox);
        }
}


void Bvh::CreateDynamicBranch()
{
        // the bvh has two main branches
        // a static branch (the old root), and adynamic branch 
        // we create a 'dynamic' leaf which basically is a container
        // for all dynamic objects underneath

        // the bounding boxes of the dynamic tree must be updated
        // once each frame in order to be able to incorporate
        // the movements of the objects within

        DEL_PTR(mDynamicRoot);

        BvhLeaf *l = new BvhLeaf(mRoot);
        mDynamicRoot = l;

        l->mBox = ComputeBoundingBox(mGeometry + mStaticGeometrySize, (int)mDynamicGeometrySize);

        l->mFirst = (int)mStaticGeometrySize;
        l->mLast = (int)mGeometrySize - 1;
        l->mArea = l->mBox.SurfaceArea();
        
        cout << "updating dynamic branch " << l->mFirst << " " << l->mLast << endl;

        if (mDynamicGeometrySize)
                mDynamicRoot = SubdivideLeaf(l, 0);
}


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


void Bvh::CreateRoot()
{
        // create new root
        mRoot = new BvhInterior(NULL);

        // the separation is a purely logical one
        // the bounding boxes of the child nodes are 
        // identical to those of the root node

        mRoot->mBox = mStaticRoot->mBox;
        mRoot->mBox.Include(mDynamicRoot->mBox);

        mRoot->mArea = mRoot->mBox.SurfaceArea();

        mRoot->mFirst = 0;
        mRoot->mLast = (int)mGeometrySize - 1;

        //cout<<"f: " << mRoot->mFirst<< " l: " <<mRoot->mLast << endl;
        // add static root on left subtree
        mRoot->mFront = mStaticRoot;
        mStaticRoot->mParent = mRoot;

        // add dynamic root on left subtree
        mRoot->mBack = mDynamicRoot;
        mDynamicRoot->mParent = mRoot;
}


}