source: GTP/trunk/Lib/Vis/OnlineCullingCHC/OGRE/BvHierarchySceneManager/OgreBvHierarchy.cpp @ 1316

/*
-----------------------------------------------------------------------------
This source file is part of the GameTools Project
http://www.gametools.org

Author: Martin Szydlowski
-----------------------------------------------------------------------------
*/

#include "OgreKdTree.h"
#include "OgreKdRenderable.h"
#include "OgreKdTreeSceneNode.h"
#include "OgreKdTreeSceneManager.h"

#include <OgreStringConverter.h>
#include <OgreLogManager.h>
#include <OgreRoot.h>
#include <OgreTimer.h>

#include <OgreMaterialManager.h>

#define PLT_SIZE 101

namespace Ogre
{

enum Intersection
{
        OUTSIDE=0,
        INSIDE=1,
        INTERSECT=2
};

Intersection intersect( const Ray &one, const AxisAlignedBox &two )
{
        // Null box?
        if (two.isNull()) return OUTSIDE;

        bool inside = true;
        const Vector3* pCorners = two.getAllCorners();
        Vector3 origin = one.getOrigin();
        Vector3 dir = one.getDirection();

        Vector3 maxT(-1, -1, -1);

        int i = 0;
        for(i=0; i<3; i++ )
        {
                if( origin[i] < pCorners[0][i] )
                {
                        inside = false;
                        if( dir[i] > 0 )
                        {
                                maxT[i] = (pCorners[0][i] - origin[i])/ dir[i];
                        }
                }
                else if( origin[i] > pCorners[4][i] )
                {
                        inside = false;
                        if( dir[i] < 0 )
                        {
                                maxT[i] = (pCorners[4][i] - origin[i]) / dir[i];
                        }
                }
        }

        if( inside )
        {
                return INTERSECT;
        }
        int whichPlane = 0;
        if( maxT[1] > maxT[whichPlane])
                whichPlane = 1;
        if( maxT[2] > maxT[whichPlane])
                whichPlane = 2;

        if( ((int)maxT[whichPlane]) & 0x80000000 )
        {
                return OUTSIDE;
        }
        for(i=0; i<3; i++ )
        {
                if( i!= whichPlane )
                {
                        float f = origin[i] + maxT[whichPlane] * dir[i];
                        if ( f < (pCorners[0][i] - 0.00001f) ||
                                f > (pCorners[4][i] +0.00001f ) )
                        {
                                return OUTSIDE;
                        }
                }
        }

        return INTERSECT;

}


/** Checks how the second box intersects with the first.
*/
Intersection intersect( const PlaneBoundedVolume &one, const AxisAlignedBox &two )
{
        // Null box?
        if (two.isNull()) return OUTSIDE;

        // Get corners of the box
        const Vector3* pCorners = two.getAllCorners();

        // For each plane, see if all points are on the negative side
        // If so, object is not visible.
        // If one or more are, it's partial.
        // If all aren't, full
        int corners[ 8 ] = {0, 4, 3, 5, 2, 6, 1, 7};
        bool all_inside = true;
        PlaneList::const_iterator i, iend;
        iend = one.planes.end();
        for (i = one.planes.begin(); i != iend; ++i)
        {
                const Plane& plane = *i;
                bool all_outside = true;

                float distance = 0;

                for ( int corner = 0; corner < 8; ++corner )
                {
                        distance = plane.getDistance( pCorners[ corners[ corner ] ] );
                        all_outside = all_outside && ( distance < 0 );
                        all_inside = all_inside && ( distance >= 0 );

                        if ( !all_outside && !all_inside )
                                break;
                }

                if ( all_outside )
                        return OUTSIDE;
        }

        if ( all_inside )
                return INSIDE;
        else
                return INTERSECT;

}


/** Checks how the second box intersects with the first.
*/
Intersection intersect( const AxisAlignedBox &one, const AxisAlignedBox &two )
{
        // Null box?
        if (one.isNull() || two.isNull()) return OUTSIDE;

        const Vector3 * outside = one.getAllCorners();
        const Vector3 *inside = two.getAllCorners();

        if ( inside[ 4 ].x < outside[ 0 ].x ||
                inside[ 4 ].y < outside[ 0 ].y ||
                inside[ 4 ].z < outside[ 0 ].z ||
                inside[ 0 ].x > outside[ 4 ].x ||
                inside[ 0 ].y > outside[ 4 ].y ||
                inside[ 0 ].z > outside[ 4 ].z )
        {
                return OUTSIDE;
        }

        bool full = ( inside[ 0 ].x > outside[ 0 ].x &&
                inside[ 0 ].y > outside[ 0 ].y &&
                inside[ 0 ].z > outside[ 0 ].z &&
                inside[ 4 ].x < outside[ 4 ].x &&
                inside[ 4 ].y < outside[ 4 ].y &&
                inside[ 4 ].z < outside[ 4 ].z );

        if ( full )
                return INSIDE;
        else
                return INTERSECT;

}

/** Checks how the box intersects with the sphere.
*/
Intersection intersect( const Sphere &one, const AxisAlignedBox &two )
{
        // Null box?
        if (two.isNull()) return OUTSIDE;

        float sradius = one.getRadius();

        sradius *= sradius;

        Vector3 scenter = one.getCenter();

        const Vector3 *corners = two.getAllCorners();

        float s, d = 0;

        Vector3 mndistance = ( corners[ 0 ] - scenter );
        Vector3 mxdistance = ( corners[ 4 ] - scenter );

        if ( mndistance.squaredLength() < sradius &&
                mxdistance.squaredLength() < sradius )
        {
                return INSIDE;
        }

        //find the square of the distance
        //from the sphere to the box
        for ( int i = 0 ; i < 3 ; i++ )
        {
                if ( scenter[ i ] < corners[ 0 ][ i ] )
                {
                        s = scenter[ i ] - corners[ 0 ][ i ];
                        d += s * s;
                }

                else if ( scenter[ i ] > corners[ 4 ][ i ] )
                {
                        s = scenter[ i ] - corners[ 4 ][ i ];
                        d += s * s;
                }

        }

        bool partial = ( d <= sradius );

        if ( !partial )
        {
                return OUTSIDE;
        }

        else
        {
                return INTERSECT;
        }
}

Real PlaneEvent::KT = 2.0;
Real PlaneEvent::KI = 1.0;

//----------------------------------------------------------------------------
// determine if this event is left or right of the reference event
void PlaneEvent::classify(const PlaneEvent& e, PlaneEvent::Side side)
{
        // only classify events of the same dimension
        if (mDimension == e.mDimension)
        {
                if (mType == PET_END && mPosition[mDimension] <= e.mPosition[e.mDimension])
                {
                        mRenderable->setSide(PES_LEFT);
                }
                else if (mType == PET_START && mPosition[mDimension] >= e.mPosition[e.mDimension])
                {
                        mRenderable->setSide(PES_RIGHT);
                }
                else if (mType == PET_ON)
                {
                        if (mPosition[mDimension] < e.mPosition[e.mDimension] ||
                                (mPosition[mDimension] == e.mPosition[e.mDimension] && side == PES_LEFT))
                        {
                                mRenderable->setSide(PES_LEFT);
                        }
                        if (mPosition[mDimension] > e.mPosition[e.mDimension] ||
                                (mPosition[mDimension] == e.mPosition[e.mDimension] && side == PES_RIGHT))
                        {
                                mRenderable->setSide(PES_RIGHT);
                        }
                }
        }
}

//----------------------------------------------------------------------------
// clip this event to an aabb (move it so that the plane on one of the box planes)
PlaneEvent PlaneEvent::clip(AxisAlignedBox& box, PlaneEvent::Dimension dim)
{
        Vector3 newpos = mPosition, min = box.getMinimum(), max = box.getMaximum();
        if (newpos[dim] < min[dim])
                newpos[dim] = min[dim];
        else if (newpos[dim] > max[dim])
                newpos[dim] = max[dim];

        return PlaneEvent(mRenderable, newpos, mDimension, mType);
}

//----------------------------------------------------------------------------
// the surface area heuristic to determine the cost of splitting the parent aabb
// along the plane represented by this event
void PlaneEvent::SAH(const AxisAlignedBox& parent, int nLeft, int nPlane, int nRight, SplitInfo& split)
{
        Real mu = splitBox(parent, split.bleft, split.bright);
        Real sav = surfaceArea(parent);
        Real pl = surfaceArea(split.bleft) / sav;
        Real pr = surfaceArea(split.bright) / sav;
        Real costl = splitCost(pl, pr, nLeft + nPlane, nRight, mu);
        Real costr = splitCost(pl, pr, nLeft, nPlane + nRight, mu);

        if (costl < costr)
        {
                split.cost = costl;
                split.side = PES_LEFT;
                split.nleft = nLeft + nPlane;
                split.nright = nRight;
        }
        else
        {
                split.cost = costr;
                split.side = PES_RIGHT;
                split.nleft = nLeft;
                split.nright = nPlane + nRight;

        }
        split.event = *this;
}

//----------------------------------------------------------------------------
// split the parent aabb with the plane in this event
Real PlaneEvent::splitBox(const AxisAlignedBox& parent, AxisAlignedBox& left, AxisAlignedBox& right)
{
        Vector3 bmin = parent.getMinimum();
        Vector3 bmax = parent.getMaximum();
        // calculate the penalty for spliting the box that way
        Real mu = lookupPenalty(
                (mPosition[mDimension] - bmin[mDimension]) / 
                (bmax[mDimension] - bmin[mDimension]));
        // set corners which are the same as parent AABB
        left.setMinimum(bmin);
        right.setMaximum(bmax);
        // modify "inner" corners
        bmin[mDimension] = mPosition[mDimension];
        bmax[mDimension] = mPosition[mDimension];
        // set the corners on the split plane
        left.setMaximum(bmax);
        right.setMinimum(bmin);

        return mu;
}

//----------------------------------------------------------------------------
// compute surface area of a box ... DUH!
Real PlaneEvent::surfaceArea(const AxisAlignedBox& box)
{
        Vector3 sides = box.getMaximum() - box.getMinimum();
        return  2 * sides.x * sides.y +
                2 * sides.y * sides.z +
                2 * sides.z * sides.x;
}

//----------------------------------------------------------------------------
// lookup the penalty for placing the splitting plane near to the edge of the AABB
// 0.0 <= p <= 1.0, p = 0.5 means the plane divides the aabb in half
Real PlaneEvent::lookupPenalty(Real p)
{
        // precomputed table of {x^6 + 1|0 <= x <= 1}
        static Real mPenaltyLookupTable[PLT_SIZE];
        static bool init_done = false;

        if (!init_done)
        {
                Real step = 1.0  / (PLT_SIZE - 1);
                Real x = 0.0; 
                //LogManager::getSingleton().logMessage("### Calculating Lookup Table ###");
                for (int i = 0; i < PLT_SIZE; i++)
                {
                        mPenaltyLookupTable[i] = Math::Pow(x, 6) + 1.0;
                        x += step;
                        //LogManager::getSingleton().logMessage("### mPenaltyLookupTable[" + StringConverter::toString(i,3) + "]=" + StringConverter::toString(mPenaltyLookupTable[i]));
                }
                init_done = true;
                //LogManager::getSingleton().logMessage("### Lookup Table Calculated ###");
        }

        // normalize p to [0,1]
        Real x = Math::Abs(p * 2 - 1);
        // compute index
        int i = Math::IFloor(x * (PLT_SIZE - 1));

        return mPenaltyLookupTable[i];
}

//----------------------------------------------------------------------------
// compute cost of the split, reward splitting of empty space (lambda, const),
// penalize splitting off 'thin' slices (mu, const)
Real PlaneEvent::splitCost(Real pLeft, Real pRight, int nLeft, int nRight)
{
        // reward splitting off chunks of empty space
        Real lambda = 1.0;
        if (nLeft == 0 || nRight == 0)
        {
                lambda = 0.8;
        }

        // penalize splitting off small chunks (thin slices)
        Real mu = 1.0;
        if (pLeft < 0.1 || pRight < 0.1 || pLeft > 0.9 || pRight > 0.9)
        {
                mu = 1.5;
        }
        return lambda * mu * (KT + (KI * (pLeft*nLeft + pRight*nRight)));
}

//----------------------------------------------------------------------------
// compute cost of the split, reward splitting of empty space (lambda, const),
// penalize splitting off 'thin' slices (mu, parameter)
Real PlaneEvent::splitCost(Real pLeft, Real pRight, int nLeft, int nRight, Real mu)
{
        // reward splitting off chunks of empty space
        Real lambda = 1.0;
        if (nLeft == 0 || nRight == 0)
        {
                lambda = 0.8;
        }

        return lambda * mu * (KT + (KI * (pLeft*nLeft + pRight*nRight)));
}

//----------------------------------------------------------------------------
// surface area heuristic modified for the priority queue method of building
// the probabilities (p, pl, pr) are relative to the global (all enclosing) aabb
void PlaneEvent::pqSAH(Real globalSA, Real parentSA, int nLeft, int nPlane, int nRight, 
        AxisAlignedBox& parentBox, SplitInfo& split)
{
        Real mu = splitBox(parentBox, split.bleft, split.bright);
        Real p = parentSA / globalSA;
        Real pl = surfaceArea(split.bleft) / globalSA;
        Real pr = surfaceArea(split.bright) / globalSA;
        Real costl = pqSplitCost(p, pl, pr, nLeft + nPlane, nRight, mu);
        Real costr = pqSplitCost(p, pl, pr, nLeft, nPlane + nRight, mu);

        if (costl < costr)
        {
                split.cost = costl;
                split.side = PES_LEFT;
                split.nleft = nLeft + nPlane;
                split.nright = nRight;
        }
        else
        {
                split.cost = costr;
                split.side = PES_RIGHT;
                split.nleft = nLeft;
                split.nright = nPlane + nRight;

        }
        split.event = *this;
}

//----------------------------------------------------------------------------
// compute split cost without any penalties
Real PlaneEvent::pqSplitCost(Real pParent, Real pLeft, Real pRight, int nLeft, int nRight, Real mu)
{
        return pParent * KT + (KI * (pLeft * nLeft + pRight * nRight));
}

//----------------------------------------------------------------------------
// DEBUG
String PlaneEvent::print()
{
        String dim, type;
        if (mDimension == PED_X)
                dim = "X";
        if (mDimension == PED_Y)
                dim = "Y";
        if (mDimension == PED_Z)
                dim = "Z";
        if (mType == PET_END)
                type = "END  ";
        if (mType == PET_ON)
                type = "ON   ";
        if (mType == PET_START)
                type = "START";
        //return StringConverter::toString(mPosition[mDimension]) + " " + dim + " " + type;
        return type + " " + dim + " " + StringConverter::toString(mPosition[mDimension]);
};

//----------------------------------------------------------------------------
String SplitInfo::print()
{
        String sside;
        if (side == PlaneEvent::PES_BOTH)
                sside = "BOTH ";
        if (side == PlaneEvent::PES_LEFT)
                sside = "LEFT ";
        if (side == PlaneEvent::PES_RIGHT)
                sside = "RIGHT";
        return "nLeft: " + StringConverter::toString(nleft, 4) + " nRight: " + 
                StringConverter::toString(nright, 4) + " Cost: " + StringConverter::toString(cost, 8, 9) +
                " Side: " + sside + " Event: " + event.print() + " Leftbox: " + 
                StringConverter::toString(bleft.getMinimum()) + ", " + 
                StringConverter::toString(bleft.getMaximum()) + " Rightbox: " +
                StringConverter::toString(bright.getMinimum()) + ", " + 
                StringConverter::toString(bright.getMaximum());
};

//----------------------------------------------------------------------------
String KdTree::SplitCandidate::print()
{               
        String sside;
        if (side == PlaneEvent::PES_BOTH)
                sside = "BOTH ";
        if (side == PlaneEvent::PES_LEFT)
                sside = "LEFT ";
        if (side == PlaneEvent::PES_RIGHT)
                sside = "RIGHT";
        return "List: " + StringConverter::toString(events->size(), 4) + " Objects: " +
                StringConverter::toString(nObjects, 4) + " Cost: " + StringConverter::toString(cost, 8, 9) + 
                " Decrease: " + StringConverter::toString(decrease, 8, 9) + " Side: " + sside + " Best: " +
                best->print() +  " Box: " + StringConverter::toString(aabb.getMinimum()) + ", "
                + StringConverter::toString(aabb.getMaximum());

};

//----------------------------------------------------------------------------
//----------------------------------------------------------------------------
//----------------------------------------------------------------------------

KdTree::KdTree(int maxdepth, BuildMethod bm): 
mMaxDepth(maxdepth), 
mBuildMethod(bm), 
mHiLiteLevel(0),
mShowAllBoxes(false),
mShowNodes(true),
mKdRoot(0), 
mBuildLog(0)
{
        init();
}

KdTree::KdTree(int maxdepth, BuildMethod bm, int hilite, bool allboxes, bool shownodes):
mMaxDepth(maxdepth), 
mBuildMethod(bm), 
mHiLiteLevel(hilite),
mShowAllBoxes(allboxes),
mShowNodes(shownodes),
mKdRoot(0), 
mBuildLog(0)
{
        init();
}

KdTree::~KdTree()
{
        delete mKdRoot;
}

void KdTree::init()
{
        MaterialPtr mat;
        TextureUnitState *tex;

        // init visualization materials (unlit solid green/yellow)
        mat = MaterialManager::getSingleton().getByName("KdTree/BoxHiLite");
        if (mat.isNull())
        {
                ColourValue green(0, 1, 0);
                mat = MaterialManager::getSingleton().create("KdTree/BoxHiLite", "General");
                //mat->setAmbient(green);
                //mat->setDiffuse(green);
                mat->setLightingEnabled(false);
                tex = mat->getTechnique(0)->getPass(0)->createTextureUnitState();
                tex->setColourOperationEx(LBX_SOURCE2, LBS_CURRENT, LBS_MANUAL, green, green);
        }

        mat = MaterialManager::getSingleton().getByName("KdTree/BoxViz");
        if (mat.isNull())
        {
                ColourValue yellow(1, 1, 0);
                mat = MaterialManager::getSingleton().create("KdTree/BoxViz", "General");
                //mat->setAmbient(yellow);
                //mat->setDiffuse(yellow);
                mat->setLightingEnabled(false);
                tex = mat->getTechnique(0)->getPass(0)->createTextureUnitState();
                tex->setColourOperationEx(LBX_SOURCE2, LBS_CURRENT, LBS_MANUAL, yellow, yellow);
        }

        // retrieve or create build log
        try
        {
                mBuildLog = LogManager::getSingleton().getLog(KDTREE_LOGNAME);
        }
        catch (Exception&)
        {               
                mBuildLog = LogManager::getSingleton().createLog(KDTREE_LOGNAME);
        }

        setEnhancedVis(false);
}

/************************************************************************/
/* KdTree insert/delete functions                                       */
/************************************************************************/

void KdTree::remove(KdRenderable * rend)
{
        // DEBUG
        //LogManager::getSingleton().logMessage("-- Removing SceneNode: " + (static_cast<KdTreeSceneNode *>(rend))->getName());
        std::queue<LeafPtr> cleanup;
        LeafPtr leaf;
        LeafSet ls = rend->getLeaves();
        LeafSet::iterator it = ls.begin();
        LeafSet::iterator end = ls.end();
        while (it != end)
        {
                cleanup.push(*it);
                it++;
        }
        while (!cleanup.empty())
        {
                leaf = cleanup.front();
                cleanup.pop();
                leaf->remove(rend);
                bool gone = rend->detachFrom(leaf);
                assert(gone && "Failed to detach leave which is abso-fuckin-lutely impossible!!!");
                //dump();
                recDelete(leaf);
                //dump();
        }
}

void KdTree::recDelete(KdTree::Node * node)
{
        if (node == 0) // DEBUG
        {
                OGRE_EXCEPT(Exception::ERR_INTERNAL_ERROR,
                        "SNAFU while inserting KdRenderable into KdTree.",
                        "KdTree::recInsert" );
                return;
        }

        if (node->isEmpty())
        {
                if (node == mKdRoot)
                {
                        OGRE_DELETE(mKdRoot);
                } 
                else
                {
                        KdTree::Branch * parent = KDBRANCHPTR_CAST(node->getParent());
                        if (node == parent->mLeft)
                        {
                                OGRE_DELETE(parent->mLeft);
                        }
                        else if (node == parent->mRight)
                        {
                                OGRE_DELETE(parent->mRight);
                        }
                        recDelete(parent);
                }
        }
}

void KdTree::insert(KdRenderable * rend)
{
        // make sure the tree exists
        if (mKdRoot)
        {
                // make sure the renderable is inside the world bounding box
                AxisAlignedBox aabb = rend->getBoundingBox();
                AxisAlignedBox isect = mKdRoot->mAABB.intersection(aabb);
                if (isect.getMinimum() == aabb.getMinimum() && isect.getMaximum() == aabb.getMaximum())
                {
                        recInsert(mKdRoot, rend);
                }
                else
                {
                        //LogManager::getSingleton().logMessage("Inserted node outside of world AABB.");
                        KdRenderableList nodelist;
                        nodelist.push_back(rend);
                        addRendToList(mKdRoot, nodelist);
                        OGRE_DELETE(mKdRoot);
                        mKdRoot = buildFromList(nodelist, 0, AxisAlignedBox());
                }
        }
        // otherwise build a new one
        else
        {
                build(rend);
        }
}

void KdTree::recInsert(KdTree::Node * node, KdRenderable * rend)
{
        if (node == 0) // DEBUG
        {
                OGRE_EXCEPT(Exception::ERR_INTERNAL_ERROR,
                        "SNAFU while inserting KdRenderable into KdTree.",
                        "KdTree::recInsert" );
                return;
        }

        // rebuild the leaf/replace with subtree ...
        if (node->isLeaf())
        {
                //LogManager::getSingleton().logMessage("## Replacing leaf.");
                rebuildSubtree(node, rend);
        }
        else
        {
                KdTree::Branch * branch = KDBRANCHPTR_CAST(node);
                AxisAlignedBox aabb = rend->getBoundingBox();
                Plane::Side smin = branch->mSplitPlane->getSide(aabb.getMinimum());
                Plane::Side smax = branch->mSplitPlane->getSide(aabb.getMaximum());
                if (smin == smax)
                {
                        if (smax == Plane::NEGATIVE_SIDE ||
                                (smax == Plane::NO_SIDE && branch->mPlaneSide == PlaneEvent::PES_LEFT))
                        {
                                if (branch->mLeft)
                                        recInsert(branch->mLeft, rend);
                                else
                                        recInsertNew(branch, PlaneEvent::PES_LEFT, rend);
                        }
                        else if (smin == Plane::POSITIVE_SIDE || 
                                (smin == Plane::NO_SIDE && branch->mPlaneSide == PlaneEvent::PES_RIGHT))
                        {
                                if (branch->mRight)
                                        recInsert(branch->mRight, rend);
                                else
                                        recInsertNew(branch, PlaneEvent::PES_RIGHT, rend);
                        }
                }
                else
                {
                        if (smin == Plane::NEGATIVE_SIDE && smax == Plane::NO_SIDE)
                        {
                                if (branch->mLeft)
                                        recInsert(branch->mLeft, rend);
                                else
                                        recInsertNew(branch, PlaneEvent::PES_LEFT, rend);
                        }
                        else if (smax == Plane::POSITIVE_SIDE && smin == Plane::NO_SIDE)
                        {
                                if (branch->mRight)
                                        recInsert(branch->mRight, rend);
                                else
                                        recInsertNew(branch, PlaneEvent::PES_RIGHT, rend);
                        }
                        else
                        {
                                // to avoid SNAFUs, rebuild whole subtree
                                //LogManager::getSingleton().logMessage("## Rebuilding subtree for insertion");
                                rebuildSubtree(node, rend);
                        }
                }
        }
}

void KdTree::recInsertNew(KdTree::Branch * parent, PlaneEvent::Side side, KdRenderable * rend)
{
        //LogManager::getSingleton().logMessage("## Inserting into new subtree");

        AxisAlignedBox left, rigth, aabb;
        PlaneEventList events;
        int nObjects;
        
        rend->computeScene(events, aabb, nObjects, false);

        // override aabb
        splitBox(*parent, left, rigth);
        if (side == PlaneEvent::PES_LEFT)
        {
                aabb = left;
                if (mBuildMethod == KDBM_RECURSIVE)
                        parent->mLeft = recBuild(events, nObjects, aabb, parent);
                else if (mBuildMethod == KDBM_PRIORITYQUEUE)
                        parent->mLeft = pqBuild(events, nObjects, aabb, parent);
                else
                {
                        OGRE_EXCEPT(Exception::ERR_INVALIDPARAMS, 
                                "Invalid build method for the KdTree.",
                                "KdTree::buildFromList");
                        parent->mLeft = 0;
                }
                
        }
        else if (side == PlaneEvent::PES_RIGHT)
        {
                aabb = rigth;
                if (mBuildMethod == KDBM_RECURSIVE)
                        parent->mRight = recBuild(events, nObjects, aabb, parent);
                else if (mBuildMethod == KDBM_PRIORITYQUEUE)
                        parent->mRight = pqBuild(events, nObjects, aabb, parent);
                else
                {
                        OGRE_EXCEPT(Exception::ERR_INVALIDPARAMS, 
                                "Invalid build method for the KdTree.",
                                "KdTree::buildFromList");
                        parent->mRight = 0;
                }
        }
}

void KdTree::rebuildSubtree(KdTree::Node * node, KdRenderable * rend)
{
        //LogManager::getSingleton().logMessage("## Rebuilding subtree");

        AxisAlignedBox aabb = node->mAABB;
        
        KdRenderableList nodelist;
        nodelist.push_back(rend);
        addRendToList(node, nodelist);

        if (node == mKdRoot)
        {
                OGRE_DELETE(mKdRoot);
                mKdRoot = buildFromList(nodelist, 0, aabb);
        }
        else
        {
                KdTree::Branch * parent = KDBRANCHPTR_CAST(node->getParent());

                if (node == parent->mLeft)
                {
                        OGRE_DELETE(parent->mLeft);
                        parent->mLeft = buildFromList(nodelist, parent, aabb);
                }
                else if (node == parent->mRight)
                {
                        OGRE_DELETE(parent->mRight);
                        parent->mRight = buildFromList(nodelist, parent, aabb);
                }
                else
                {
                        OGRE_EXCEPT(Exception::ERR_INTERNAL_ERROR,
                                "SNAFU while inserting KdRenderable into KdTree.",
                                "KdTree::recInsert" );
                }
        }
}

// compute both AABB then return the requested one.
void KdTree::splitBox(const KdTree::Branch& parent, AxisAlignedBox& left, AxisAlignedBox& right)
{
        Vector3 bmin = parent.mAABB.getMinimum();
        Vector3 bmax = parent.mAABB.getMaximum();
        Real pos = - parent.mSplitPlane->d;
        int dim = static_cast<int>(parent.mSplitPlane->normal.dotProduct(Vector3(0,1,2)));
        // set corners which are the same as parent AABB
        left.setMinimum(bmin);
        right.setMaximum(bmax);
        // modify "inner" corners
        bmin[dim] = pos;
        bmax[dim] = pos;
        // set the corners on the split plane
        left.setMaximum(bmax);
        right.setMinimum(bmin);
}

void KdTree::addRendToList(KdTree::Node * node, KdRenderableList& nodelist)
{
        if (node->isLeaf())
        {
                KdTree::Leaf * leaf = KDLEAFPTR_CAST(node);
                KdRenderableList::iterator it  = leaf->mKdRenderables.begin();
                KdRenderableList::iterator end = leaf->mKdRenderables.end();
                while (it != end)
                {
                        nodelist.push_back(*it);
                        it++;
                }
        }
        else
        {
                KdTree::Branch * branch = KDBRANCHPTR_CAST(node);
                if (branch->mLeft)
                        addRendToList(branch->mLeft, nodelist);
                if (branch->mRight)
                        addRendToList(branch->mRight, nodelist);
        }
}

/************************************************************************/
/* KdTree build functions                                               */
/************************************************************************/

void KdTree::build(KdRenderable * sceneRoot)
{
        Timer *timer = Root::getSingleton().getTimer();
        unsigned long t1, t2, t3, t4;

        mTreeStats.clear();
        
        // data we want to collect
        PlaneEventList events;
        AxisAlignedBox aabb;
        int nObjects = 0;

        t1 = timer->getMicroseconds(); // DEBUG
        sceneRoot->computeScene(events, aabb, nObjects);
        t2 = timer->getMicroseconds(); // DEBUG
        events.sort();
        t3 = timer->getMicroseconds(); // DEBUG

        mTreeStats.mNumSceneNodes = nObjects;

        assert(! aabb.isNull() && "Teh stubid worldAABB iz NULL ... waht now?");
        // hack to avoid SNAFU with "2-dimensional" scene
        aabb.merge(aabb.getMaximum()+Vector3(1,1,1));
        aabb.merge(aabb.getMinimum()+Vector3(-1,-1,-1));

        if (mBuildMethod == KDBM_RECURSIVE)
                mKdRoot = recBuild(events, nObjects, aabb, 0);
        else if (mBuildMethod == KDBM_PRIORITYQUEUE)
                mKdRoot = pqBuild(events, nObjects, aabb, 0);
        else
        {
                OGRE_EXCEPT(Exception::ERR_INVALIDPARAMS, 
                        "Invalid build method for the KdTree.",
                        "KdTree::build");
                mKdRoot = 0;
        }

        t4 = timer->getMicroseconds(); // DEBUG

        String method = "Invalid";
        if (mBuildMethod == KDBM_RECURSIVE)
                method = "Recursive";
        else if (mBuildMethod == KDBM_PRIORITYQUEUE)
                method = "Priority Queue";

        mBuildLog->logMessage("######## SAH Statistics ########");
        mBuildLog->logMessage("Build Method: " + method);
        mBuildLog->logMessage("Time for events build: " + StringConverter::toString(t2 - t1) + "µs");
        mBuildLog->logMessage("Time for events sort: " + StringConverter::toString(t3 - t2) + "µs");
        mBuildLog->logMessage("Time for tree build: " + StringConverter::toString(t4 - t3) + "µs");
        mBuildLog->logMessage("Total time: " + StringConverter::toString(t4 - t1) + "µs");
        mBuildLog->logMessage("Tree Depth: " + StringConverter::toString(mMaxDepth));
        mBuildLog->logMessage("Number of Objects: " + StringConverter::toString(mTreeStats.mNumSceneNodes));
        mBuildLog->logMessage("Number of Leaves: " + StringConverter::toString(mTreeStats.mNumLeaves));
        mBuildLog->logMessage("Number of Nodes: " + StringConverter::toString(mTreeStats.mNumNodes));
        mBuildLog->logMessage("Total cost: " + StringConverter::toString(calcCost()));
        mBuildLog->logMessage("################################");
}

KdTree::Node * KdTree::buildFromList(KdRenderableList& nodelist, KdTree::Branch * parent, AxisAlignedBox& aabb)
{
        // data we want to collect
        PlaneEventList events;
        AxisAlignedBox nodeaabb;
        int nObjects = 0;

        KdRenderableList::iterator it = nodelist.begin();
        KdRenderableList::iterator end = nodelist.end();
        while (it != end)
        {
                (*it)->computeScene(events, nodeaabb, nObjects, false);
                it++;
        }

        events.sort();
        
        // HACK
        if (aabb.isNull())
        {
                aabb = nodeaabb;
        }

        if (mBuildMethod == KDBM_RECURSIVE)
                return recBuild(events, nObjects, aabb, parent);
        else if (mBuildMethod == KDBM_PRIORITYQUEUE)
                return pqBuild(events, nObjects, aabb, parent);
        else
        {
                OGRE_EXCEPT(Exception::ERR_INVALIDPARAMS, 
                        "Invalid build method for the KdTree.",
                        "KdTree::buildFromList");
                return 0;
        }
}

KdTree::Node * KdTree::recBuild(PlaneEventList& events, int nObjects, AxisAlignedBox& aabb, KdTree::Branch * parent)
{
        // determine the depth we are on
        int level = 0;
        if (parent)
        {
                level = parent->getLevel() + 1;
        }

        /************************************************/
        /**************** BEGIN FINDPLANE ***************/
        /************************************************/
        // find optimal split plane and split node accordingly

        // initialize
        const int dim = 3;
        int pStart, pOn, pEnd;
        int nLeft[dim], nPlane[dim], nRight[dim];
        for (int i = 0; i < dim; i++)
        {
                nLeft[i] = 0; nPlane[i] = 0; nRight[i] = nObjects;
        }

        SplitInfo split, best;
        best.cost = Math::POS_INFINITY;

        PlaneEventList::iterator begin = events.begin();
        PlaneEventList::iterator end = events.end();
        PlaneEventList::iterator it = begin;
        // try all planes to find the best one for the given set of objects
        while (it != end)
        {
                PlaneEventList::iterator p = it;
                pStart = pOn = pEnd = 0;
                while (it != end && p->equalsType(*it, PlaneEvent::PET_END))
                {
                        it++; pEnd++;
                }
                while (it != end && p->equalsType(*it, PlaneEvent::PET_ON))
                {
                        it++; pOn++;
                }
                while (it != end && p->equalsType(*it, PlaneEvent::PET_START))
                {
                        it++; pStart++;
                }
                int d = p->getDimension();
                nPlane[d] = pOn; nRight[d] -= pOn; nRight[d] -= pEnd;
                p->SAH(aabb, nLeft[d], nPlane[d], nRight[d], split);
                if (split.cost < best.cost)
                {
                        best = split;
                }
                nLeft[d] += pStart; nLeft[d] += pOn; nPlane[d] = 0;
        }

        /************************************************/
        /**************** BEGIN TERMINATE ***************/
        /************************************************/
        // check terminating condition
        if (best.cost > PlaneEvent::KI*nObjects || level >= mMaxDepth)
        {
                // Terminating condition reached, create leaf and add renderables to list
                KdTree::Leaf * leaf = new KdTree::Leaf(this, level, aabb, parent);
                KdRenderable *rend;
                it = begin;
                while (it != end)
                {
                        rend = it->getRenderable();
                        rend->setClassified(false);
                        if (rend->attachTo(leaf, this))
                        {
                                leaf->insert(rend);
                        }
                        it++;
                }
                // update stats
                ++ mTreeStats.mNumNodes;
                ++ mTreeStats.mNumLeaves;
                // update bounding box
                leaf->_updateBounds(false);
                return leaf;
        }

        /************************************************/
        /**************** BEGIN CLASSIFY ****************/
        /************************************************/
        // split the event list in left and right sub-lists
        else
        {
                PlaneEventList eventsRight, eventsLeft;
                it = begin;
                // slightly redundant, since we mark each renderable up to 6 times
                while (it != end)
                {
                        it->getRenderable()->setSide(PlaneEvent::PES_BOTH);
                        it->getRenderable()->setClassified(false);
                        it++;
                }
                // now classify all renderables. do they belong to the left child, the right child or both?
                it = begin;
                while (it != end)
                {
                        it->classify(best.event, best.side);
                        it++;
                }
                // divide the event lists
                int nLeftS = 0, nRightS = 0, nBothS = 0;
                it = begin;
                while (it != end)
                {
                        // right-only nodes go in right list
                        if (it->getRenderable()->getSide() == PlaneEvent::PES_RIGHT)
                        {
                                if (!it->getRenderable()->isClassified())
                                {
                                        it->getRenderable()->setClassified(true);
                                        nRightS++;
                                }
                                eventsRight.push_back(*it);
                        }
                        // left-only nodes go in left list
                        else if (it->getRenderable()->getSide() == PlaneEvent::PES_LEFT)
                        {
                                if (!it->getRenderable()->isClassified())
                                {
                                        it->getRenderable()->setClassified(true);
                                        nLeftS++;
                                }
                                eventsLeft.push_back(*it);
                        }
                        // remaining nodes go in both lists, bust must be clipped to prevent
                        // events from lying outside the new nodes aabb
                        else
                        {
                                if (!it->getRenderable()->isClassified())
                                {
                                        it->getRenderable()->setClassified(true);
                                        nBothS++;
                                }
                                eventsRight.push_back(it->clip(best.bright, best.event.getDimension()));
                                eventsLeft.push_back(it->clip(best.bleft, best.event.getDimension()));
                        }
                        it++;
                }

                // create a new branch node and continue recursion
                KdTree::Branch * branch = new KdTree::Branch(this, level, aabb, parent, 
                        best.event.getSplitPlane(), best.side);

                // now create the child nodes and continue recursion
                if (eventsLeft.size() > 0)
                {
                        branch->mLeft = recBuild(eventsLeft, nLeftS + nBothS, best.bleft, branch);
                }
                if (eventsRight.size() > 0)
                {
                        branch->mRight = recBuild(eventsRight, nBothS + nRightS, best.bright, branch);
                }

                // update stats
                ++ mTreeStats.mNumNodes;

                // update bounding box
                branch->_updateBounds(false);

                return branch;
        }
}

KdTree::Node * KdTree::pqBuild(PlaneEventList& events, int nObjects, AxisAlignedBox& aabb, KdTree::Branch * parent)
{
        SplitCandidatePQ pqueue;
        Real globalTreeCost;
        Real globalSA;

        KdTree::Node * newNode = 0, * topNode = 0;
        // init global tree cost
        globalTreeCost = PlaneEvent::KI * nObjects;
        globalSA = PlaneEvent::surfaceArea(aabb);

        PlaneEventList * e = new PlaneEventList(events);

        // inital split candidate
        SplitInfo * best = pqFindPlane(e, nObjects, aabb, globalSA);
        SplitCandidate splitcandidate(e, nObjects, aabb, parent, globalTreeCost, 
                globalTreeCost - best->cost, best, PlaneEvent::PES_BOTH);
        pqueue.push(splitcandidate);


        /*** BEGIN ITERATIVE BUILD ***/
        while (!pqueue.empty())
        {
                SplitCandidate sc = pqueue.top();
                pqueue.pop();

                // determine the depth we are on
                int level = 0;
                if (sc.parent)
                {
                        level = sc.parent->getLevel() + 1;
                }

                /************************************************/
                /**************** BEGIN TERMINATE ***************/
                /************************************************/
                // check terminating condition
                if (sc.decrease < 0.0 || level >= mMaxDepth)
                {
                        // Terminating condition reached, create leaf and add renderables to list
                        KdTree::Leaf * leaf = new KdTree::Leaf(this, level, sc.aabb, sc.parent);
                        KdRenderable *rend;
                        PlaneEventList::iterator begin = sc.events->begin();
                        PlaneEventList::iterator end = sc.events->end();
                        PlaneEventList::iterator it = begin;
                        while (it != end)
                        {
                                rend = it->getRenderable();
                                rend->setClassified(false);
                                if (rend->attachTo(leaf, this))
                                {
                                        leaf->insert(rend);
                                }
                                it++;
                        }
                        newNode = leaf;
                        if (!topNode)
                                topNode = leaf;
                        // update stats
                        ++ mTreeStats.mNumNodes;
                        ++ mTreeStats.mNumLeaves;
                }

                /************************************************/
                /**************** BEGIN CLASSIFY ****************/
                /************************************************/
                else
                {
                        PlaneEventList * eventsLeft = new PlaneEventList();
                        PlaneEventList * eventsRight = new PlaneEventList();
                        PlaneEventList::iterator begin = sc.events->begin();
                        PlaneEventList::iterator end = sc.events->end();
                        PlaneEventList::iterator it = begin;
                        // slightly redundant, since we mark each renderable up to 6 times
                        while (it != end)
                        {
                                it->getRenderable()->setSide(PlaneEvent::PES_BOTH);
                                it->getRenderable()->setClassified(false);
                                it++;
                        }

                        // now classify all renderables. do they belong to the left child, the right child or both?
                        it = begin;
                        while (it != end)
                        {
                                it->classify(sc.best->event, sc.best->side);
                                it++;
                        }

                        // divide the event lists
                        int nLeftS = 0, nRightS = 0, nBothS = 0;
                        it = begin;
                        while (it != end)
                        {
                                // right-only events go in the right list
                                if (it->getRenderable()->getSide() == PlaneEvent::PES_RIGHT)
                                {
                                        if (!it->getRenderable()->isClassified())
                                        {
                                                it->getRenderable()->setClassified(true);
                                                nRightS++;
                                        }

                                        eventsRight->push_back(*it);
                                }
                                // left-only events go in the left list
                                else if (it->getRenderable()->getSide() == PlaneEvent::PES_LEFT)
                                {
                                        if (!it->getRenderable()->isClassified())
                                        {
                                                it->getRenderable()->setClassified(true);
                                                nLeftS++;
                                        }
                                        eventsLeft->push_back(*it);
                                }
                                // the rest goes in both lists after being clipped
                                else
                                {
                                        if (!it->getRenderable()->isClassified())
                                        {
                                                it->getRenderable()->setClassified(true);
                                                nBothS++;
                                        }

                                        eventsRight->push_back(it->clip(sc.best->bright, sc.best->event.getDimension()));
                                        eventsLeft->push_back(it->clip(sc.best->bleft, sc.best->event.getDimension()));
                                }
                                it++;
                        }

                        // create a new branch node
                        KdTree::Branch * branch = new KdTree::Branch(this, level, sc.aabb, sc.parent, 
                                sc.best->event.getSplitPlane(), sc.best->side);

                        globalTreeCost -= sc.decrease;


                        // now for each potential child node, compute the split plane and the cost decrease
                        // and place them in the queue
                        if (eventsLeft->size() > 0)
                        {
                                best = pqFindPlane(eventsLeft, nLeftS + nBothS, sc.best->bleft, globalSA);
                                Real old = PlaneEvent::surfaceArea(sc.best->bleft)/globalSA*PlaneEvent::KI*(nLeftS + nBothS);
                                SplitCandidate scleft(eventsLeft, nLeftS + nBothS, sc.best->bleft, 
                                        branch, old, old - best->cost, best, PlaneEvent::PES_LEFT);
                                pqueue.push(scleft);
                        }
                        // cleanup
                        else
                        {
                                delete eventsLeft;
                        }

                        if (eventsRight->size() > 0)
                        {
                                best = pqFindPlane(eventsRight, nBothS + nRightS, sc.best->bright, globalSA);
                                Real old = PlaneEvent::surfaceArea(sc.best->bright)/globalSA*PlaneEvent::KI*(nBothS + nRightS);
                                SplitCandidate scright(eventsRight, nBothS + nRightS, sc.best->bright, 
                                        branch, old, old - best->cost, best, PlaneEvent::PES_RIGHT);
                                pqueue.push(scright);
                        }
                        // cleanup
                        else
                        {
                                delete eventsRight;
                        }

                        newNode = branch;
                        if (!topNode)
                                topNode = branch;


                        // update stats
                        ++ mTreeStats.mNumNodes;
                }

                // attach new node to parent
                if (sc.parent)
                {
                        if (sc.side == PlaneEvent::PES_LEFT)
                        {
                                sc.parent->mLeft = newNode;
                        }
                        else if (sc.side == PlaneEvent::PES_RIGHT)
                        {
                                sc.parent->mRight = newNode;
                        }
                        else
                        {
                                OGRE_EXCEPT(Exception::ERR_INTERNAL_ERROR,
                                        "PQBUILD SNAFU - CHILD I AM NOT LEFT NOR RIGHT","KdTree::pqBuild");
                        }
                }

                // update bounding boxes when geometry (leaf) was added
                if (newNode->isLeaf())
                        newNode->_updateBounds();

                //cleanup
                OGRE_DELETE(sc.events);
                OGRE_DELETE(sc.best);
        }
        mBuildLog->logMessage("Cost after PQBUILD €" + StringConverter::toString(globalTreeCost));
        return topNode;
}

//-------------------------------------------------------------------------
SplitInfo * KdTree::pqFindPlane(PlaneEventList * events, int nObjects, AxisAlignedBox& aabb, Real globalSA)
{
        static const int dim = 3;
        int pStart, pOn, pEnd;
        int nLeft[dim], nPlane[dim], nRight[dim];
        for (int i = 0; i < dim; i++)
        {
                nLeft[i] = 0; nPlane[i] = 0; nRight[i] = nObjects;
        }

        SplitInfo split, best;
        best.cost = Math::POS_INFINITY;

        Real parentSA = PlaneEvent::surfaceArea(aabb);

        PlaneEventList::iterator begin = events->begin();
        PlaneEventList::iterator end = events->end();
        PlaneEventList::iterator it = begin;
        // try all planes to find the best one for the given set of objects
        while (it != end)
        {
                PlaneEventList::iterator p = it;
                pStart = pOn = pEnd = 0;
                while (it != end && p->equalsType(*it, PlaneEvent::PET_END))
                {
                        it++; pEnd++;
                }
                while (it != end && p->equalsType(*it, PlaneEvent::PET_ON))
                {
                        it++; pOn++;
                }
                while (it != end && p->equalsType(*it, PlaneEvent::PET_START))
                {
                        it++; pStart++;
                }
                int d = p->getDimension();
                nPlane[d] = pOn; nRight[d] -= pOn; nRight[d] -= pEnd;
                p->pqSAH(globalSA, parentSA, nLeft[d], nPlane[d], nRight[d], aabb, split);
                if (split.cost < best.cost)
                {
                        best = split;
                }
                nLeft[d] += pStart; nLeft[d] += pOn; nPlane[d] = 0;
        }
        return new SplitInfo(best);
}

/************************************************************************/
/* KdTree rendering functions                                           */
/************************************************************************/

//-------------------------------------------------------------------------
void KdTree::queueVisibleObjects(KdTreeCamera* cam, RenderQueue* queue, bool onlyShadowCasters, 
        bool showBoxes, KdTree::NodeList& visibleNodes)
{
        mFrameStats.clear();
        cam->mNumVisQueries = 0;

        if (mKdRoot)
                recQueueVisibleObjects(mKdRoot, Root::getSingleton().getCurrentFrameNumber(),
                        cam, queue, onlyShadowCasters, showBoxes, visibleNodes);

        mFrameStats.mTraversedNodes = cam->mNumVisQueries;
}

//-------------------------------------------------------------------------
void KdTree::recQueueVisibleObjects(KdTree::Node * node, unsigned long currentFrame, 
        KdTreeCamera* cam, RenderQueue* queue, bool onlyShadowCasters, bool showBoxes, 
        KdTree::NodeList& visibleNodes, bool fullVis)
{
        KdTreeCamera::NodeVisibility vis = KdTreeCamera::KDNV_PART;
        // test visibility
        //if (cam->isVisible(node->mAABB))
        if (fullVis || 
                ((vis = (cam->*getVisibility)(node->mAABB)) != KdTreeCamera::KDNV_NONE))
        {
                visibleNodes.push_back(node);

                bool v = (fullVis || vis == KdTreeCamera::KDNV_FULL);

                node->queueVisibleObjects(currentFrame, cam, queue, onlyShadowCasters, showBoxes, v);

                if (v || node->isLeaf())
                        ++ mFrameStats.mRenderedNodes;

                if (!v)
                {

                        if (node->getLeftChild())
                                recQueueVisibleObjects(node->getLeftChild(), currentFrame, 
                                cam, queue, onlyShadowCasters, showBoxes, visibleNodes, v);
                        if (node->getRightChild())
                                recQueueVisibleObjects(node->getRightChild(), currentFrame, 
                                cam, queue, onlyShadowCasters, showBoxes, visibleNodes, v);
                }
        }
        else
        {
                ++ mFrameStats.mFrustumCulledNodes;
        }
}

//-------------------------------------------------------------------------
void KdTree::setEnhancedVis(bool enh)
{
        if (enh)
                getVisibility = &KdTreeCamera::getVisibilityEnhanced;
        else
                getVisibility = &KdTreeCamera::getVisibilitySimple;
}

//-------------------------------------------------------------------------
bool KdTree::getEnhancedVis()
{
        return getVisibility == &KdTreeCamera::getVisibilityEnhanced;
}

//-------------------------------------------------------------------------
//void KdTree::findVisibleNodes(NodeList& visibleNodes, Camera * cam)
//{
//      if (mKdRoot)
//              recFindVisibleNodes(mKdRoot, visibleNodes, cam);
//}

////-------------------------------------------------------------------------
//void KdTree::recFindVisibleNodes(KdTree::Node * node, NodeList& visibleNodes, Camera * cam)
//{
//      // test visibility
//      if (cam->isVisible(node->mAABB))
//      {
//              visibleNodes.push_back(node);

//              if (node->getLeftChild())
//                      recFindVisibleNodes(node->getLeftChild(), visibleNodes, cam);
//              if (node->getRightChild())
//                      recFindVisibleNodes(node->getRightChild(), visibleNodes, cam);
//      }
//}

void KdTree::findNodesIn(const AxisAlignedBox &box, std::list<SceneNode *> &list, SceneNode *exclude)
{
        if (mKdRoot)
                recFindNodesIn(box, list, exclude, mKdRoot);
}

void KdTree::findNodesIn(const Sphere &sphere, std::list<SceneNode *> &list, SceneNode *exclude)
{
        if (mKdRoot)
                recFindNodesIn(sphere, list, exclude, mKdRoot);
}

void KdTree::findNodesIn(const PlaneBoundedVolume &volume, std::list<SceneNode *> &list, SceneNode *exclude)
{
        if (mKdRoot)
                recFindNodesIn(volume, list, exclude, mKdRoot);
}

void KdTree::findNodesIn(const Ray &ray, std::list<SceneNode *> &list, SceneNode *exclude)
{
        if (mKdRoot)
                recFindNodesIn(ray, list, exclude, mKdRoot);
}

void KdTree::recFindNodesIn(const AxisAlignedBox &box, std::list<SceneNode *> &list, SceneNode *exclude, Node * node, bool full)
{
        // check intersection
        if ( !full )
        {
                Intersection isect = intersect(box, node->_getWorldAABB());

                if ( isect == OUTSIDE )
                        return ;

                full = ( isect == INSIDE );
        }

        if (node->isLeaf())
        {
                LeafPtr leaf = KDLEAFPTR_CAST(node);
                for (KdRenderableList::iterator it = leaf->mKdRenderables.begin();
                        it != leaf->mKdRenderables.end(); it ++)
                {
                        SceneNode *sn = dynamic_cast<SceneNode *>(*it);

                        if (sn != exclude)
                        {
                                if (full)
                                {
                                        list.push_back(sn);
                                }
                                else
                                {
                                        Intersection nsect = intersect(box, sn->_getWorldAABB());

                                        if ( nsect != OUTSIDE )
                                        {
                                                list.push_back(sn);
                                        }
                                }
                        }
                }
        }
        else
        {
                if (node->getLeftChild())
                        recFindNodesIn(box, list, exclude, node->getLeftChild(), full);
                if (node->getRightChild())
                        recFindNodesIn(box, list, exclude, node->getRightChild(), full);
        }
}

void KdTree::recFindNodesIn(const Sphere &sphere, std::list<SceneNode *> &list, SceneNode *exclude, Node * node, bool full)
{
        // TODO
}

void KdTree::recFindNodesIn(const PlaneBoundedVolume &volume, std::list<SceneNode *> &list, SceneNode *exclude, Node * node, bool full)
{
        // TODO
}

void KdTree::recFindNodesIn(const Ray &ray, std::list<SceneNode *> &list, SceneNode *exclude, Node * node, bool full)
{
        // TODO
}

/************************************************************************/
/* KdTree debug & helper functions                                      */
/************************************************************************/

//-------------------------------------------------------------------------
void KdTree::dump()
{
        //LogManager::getSingleton().logMessage("#@#@#@#@ Dumping KdTree #@#@#@#@");
        mBuildLog->logMessage("#@#@#@#@ Dumping KdTree #@#@#@#@");
        if (mKdRoot)
                dump(mKdRoot);
}

//-------------------------------------------------------------------------
void KdTree::dump(KdTree::Node * node)
{
        //LogManager * log = LogManager::getSingletonPtr();
        KdTreeSceneNode * scenenode;
        String pad;
        int p;
        
        pad = "";
        for (p = 0; p < node->getLevel(); p++)
        {
                pad += " ";
        }

        if (node->isLeaf())
        {
                KdTree::Leaf * leaf = KDLEAFPTR_CAST(node);
                KdRenderableList::iterator it  = leaf->mKdRenderables.begin();
                KdRenderableList::iterator end = leaf->mKdRenderables.end();
                while (it != end)
                {
                        scenenode = dynamic_cast<KdTreeSceneNode *>(*it);
                        mBuildLog->logMessage(pad + "# Leaf   level " + 
                                StringConverter::toString(node->getLevel()) + 
                                " SceneNode " + scenenode->getName());
                        mBuildLog->logMessage(pad + "## Objects: " + scenenode->dumpToString());
                        it++;
                }
        }
        else
        {
                KdTree::Branch * branch = KDBRANCHPTR_CAST(node);
                if (branch->mLeft)
                {
                        mBuildLog->logMessage(pad + "# Branch level " + 
                                StringConverter::toString(node->getLevel()) + " Left Child");
                        dump(branch->mLeft);
                }
                if (branch->mRight)
                {
                        mBuildLog->logMessage(pad + "# Branch level " + 
                                StringConverter::toString(node->getLevel()) + " Right Child");
                        dump(branch->mRight);
                }
        }
}

//-------------------------------------------------------------------------
Real KdTree::calcCost()
{
        if (mKdRoot)
                return calcCost(mKdRoot, PlaneEvent::surfaceArea(mKdRoot->mAABB));
        else
                return 0;
}

//-------------------------------------------------------------------------
Real KdTree::calcCost(KdTree::Node * node, Real vs)
{
        if (node == 0)
                return 0.0;

        if (node->isLeaf())
        {
                KdTree::Leaf * leaf = KDLEAFPTR_CAST(node);
                return (PlaneEvent::surfaceArea(node->mAABB)/vs) *
                        PlaneEvent::KI * leaf->mKdRenderables.size();
        }
        else
        {
                KdTree::Branch * branch = KDBRANCHPTR_CAST(node);
                return (PlaneEvent::surfaceArea(node->mAABB)/vs) * PlaneEvent::KT + 
                        calcCost(branch->mLeft, vs) + calcCost(branch->mRight, vs);
        }
}

/************************************************************************/
/* KdTree::Node/Branch/Leaf functions                                   */
/************************************************************************/

//-------------------------------------------------------------------------
KdTree::Leaf::~Leaf()
{
        // detach all scene nodes in the case that we are rebuilding
        // the tree but not the scene
        KdRenderableList::iterator it = mKdRenderables.begin();
        KdRenderableList::iterator end = mKdRenderables.end();
        while (it != end)
        {
                (*it)->detachFrom(this);
                it++;
        }
        mKdRenderables.clear();
}

//-------------------------------------------------------------------------
void KdTree::Leaf::queueVisibleObjects(unsigned long currentFrame, 
        Camera* cam, RenderQueue* queue, bool onlyShadowCasters, bool showBoxes, bool fullVis)
{
        KdRenderableList::iterator it  = mKdRenderables.begin();
        KdRenderableList::iterator end = mKdRenderables.end();
        while (it != end)
        {                       
                if (!(*it)->isQueued(currentFrame, cam))
                {
                        (*it)->queueObjects(cam, queue, onlyShadowCasters);
                }
                it++;
        }

        if (showBoxes)
                if (mLevel == mOwner->getHiLiteLevel() || mOwner->getShowAllBoxes())
                        queue->addRenderable(getWireBoundingBox());
}

//-------------------------------------------------------------------------
void KdTree::Leaf::getGeometryList(GtpVisibility::GeometryVector *geometryList)
{
        KdRenderableList::iterator it = mKdRenderables.begin();
        KdRenderableList::iterator end = mKdRenderables.end();
        while (it != end)
        {
                (*it)->getGeometryList(geometryList);
                it++;
        }
}

//-------------------------------------------------------------------------
// update the world aabb based on the contained geometry
void KdTree::Leaf::_updateBounds(bool recurse)
{
        // reset box
        mWorldAABB.setNull();

        // merge boxes from attached geometry
        KdRenderableList::iterator it = mKdRenderables.begin();
        KdRenderableList::iterator end = mKdRenderables.end();
        while (it != end)
        {
                //(*it)->_updateBounds();
                mWorldAABB.merge((*it)->getBoundingBox());
                it++;
        }

        // update parent recursively
        if (recurse && mParent)
                mParent->_updateBounds(recurse);
}

} // namespace Ogre