/*==LICENSE==*

CyanWorlds.com Engine - MMOG client, server and tools
Copyright (C) 2011  Cyan Worlds, Inc.

This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.

Additional permissions under GNU GPL version 3 section 7

If you modify this Program, or any covered work, by linking or
combining it with any of RAD Game Tools Bink SDK, Autodesk 3ds Max SDK,
NVIDIA PhysX SDK, Microsoft DirectX SDK, OpenSSL library, Independent
JPEG Group JPEG library, Microsoft Windows Media SDK, or Apple QuickTime SDK
(or a modified version of those libraries),
containing parts covered by the terms of the Bink SDK EULA, 3ds Max EULA,
PhysX SDK EULA, DirectX SDK EULA, OpenSSL and SSLeay licenses, IJG
JPEG Library README, Windows Media SDK EULA, or QuickTime SDK EULA, the
licensors of this Program grant you additional
permission to convey the resulting work. Corresponding Source for a
non-source form of such a combination shall include the source code for
the parts of OpenSSL and IJG JPEG Library used as well as that of the covered
work.

You can contact Cyan Worlds, Inc. by email legal@cyan.com
 or by snail mail at:
      Cyan Worlds, Inc.
      14617 N Newport Hwy
      Mead, WA   99021

*==LICENSE==*/

#include "hsTypes.h"
#include "plCullTree.h"
#include "plDrawable/plSpaceTree.h"
#include "hsFastMath.h"
#include "hsColorRGBA.h"
#include "plProfile.h"

#include "plTweak.h"

#define MF_DEBUG_NORM
#ifdef MF_DEBUG_NORM

#define IDEBUG_NORMALIZE( a, b ) { hsScalar len = hsFastMath::InvSqrtAppr((a).MagnitudeSquared()); a *= len; b *= len; }

#else // MF_DEBUG_NORM
#define IDEBUG_NORMALIZE( a, b )
#endif // MF_DEBUG_NORM

//#define CULL_SMALL_TOLERANCE
#ifdef CULL_SMALL_TOLERANCE
//static const hsScalar kTolerance = 1.e-5f;
static const hsScalar kTolerance = 1.e-3f;
#else //CULL_SMALL_TOLERANCE
static const hsScalar kTolerance = 1.e-1f;
#endif // CULL_SMALL_TOLERANCE

plProfile_CreateCounter("Harvest Nodes", "Draw", HarvestNodes);

//////////////////////////////////////////////////////////////////////
// Harvest culling section.
// These are the functions used on a built tree
//////////////////////////////////////////////////////////////////////
plCullNode::plCullStatus plCullNode::ITestBoundsRecur(const hsBounds3Ext& bnd) const
{
    plCullNode::plCullStatus retVal = TestBounds(bnd);

    // No Children, what we say goes.
    if( (fOuterChild < 0) && (fInnerChild < 0) )
        return retVal;

    // No innerchild. If we cull, it's culled, else we
    // hope our outerchild culls it.
    if( fInnerChild < 0 )
    {
        if( retVal == kCulled )
            return kCulled;

        return IGetNode(fOuterChild)->ITestBoundsRecur(bnd);
    }

    // No outerchild. If we say it's clear, it's clear (or split), but if
    // it's culled, we have to pass it to innerchild, who may pronounce it clear
    if( fOuterChild < 0 )
    {
        if( retVal == kClear )
            return kClear;
        if( retVal == kSplit )
            return kSplit;
        return IGetNode(fInnerChild)->ITestBoundsRecur(bnd);
    }

    // We've got both children to feed.
    // We pass the clear ones to the inner child, culled to outer, 
    // and split to both. Remember, a both children have to agree to cull a split.
    if( retVal == kClear )
        return IGetNode(fOuterChild)->ITestBoundsRecur(bnd);

    if( retVal == kCulled )
        return IGetNode(fInnerChild)->ITestBoundsRecur(bnd);

    // Here's the split, to be culled, both children have to
    // say its culled.
    if( kCulled != IGetNode(fOuterChild)->ITestBoundsRecur(bnd) )
        return kSplit;

    if( kCulled != IGetNode(fInnerChild)->ITestBoundsRecur(bnd) )
        return kSplit;

    return kCulled;
}

plCullNode::plCullStatus plCullNode::TestBounds(const hsBounds3Ext& bnd) const
{
    // Not sure if doing a sphere test will pay off or not. Some circumstantial evidence
    // from TrueTime suggests it could very well, but I really need to do some side by
    // side timings to be sure. Still looking for some reasonably constructed real data sets. mf
#define MF_TEST_SPHERE_FIRST
#ifdef MF_TEST_SPHERE_FIRST
    hsScalar dist = fNorm.InnerProduct(bnd.GetCenter()) + fDist;
    hsScalar rad = bnd.GetRadius();
    if( dist < -rad )
        return kCulled;
    if( dist > rad )
        return kClear;
#endif // MF_TEST_SPHERE_FIRST

    hsPoint2 depth;
    bnd.TestPlane(fNorm, depth);

    const hsScalar kSafetyDist = -0.1f;
    if( depth.fY + fDist < kSafetyDist )
        return kCulled;

    if( depth.fX + fDist >= 0 )
        return kClear;

    return kSplit;
}

plCullNode::plCullStatus plCullNode::ITestSphereRecur(const hsPoint3& center, hsScalar rad) const
{
    plCullNode::plCullStatus retVal = TestSphere(center, rad);

    // No Children, what we say goes.
    if( (fOuterChild < 0) && (fInnerChild < 0) )
        return retVal;

    // No innerchild. If we cull, it's culled, else we
    // hope our outerchild culls it.
    if( fInnerChild < 0 )
    {
        if( retVal == kCulled )
            return kCulled;

        return IGetNode(fOuterChild)->ITestSphereRecur(center, rad);
    }

    // No outerchild. If we say it's clear, it's clear (or split), but if
    // it's culled, we have to pass it to innerchild, who may pronounce it clear
    if( fOuterChild < 0 )
    {
        if( retVal == kClear )
            return kClear;
        if( retVal == kSplit )
            return kSplit;
        return IGetNode(fInnerChild)->ITestSphereRecur(center, rad);
    }

    // We've got both children to feed.
    // We pass the clear ones to the inner child, culled to outer, 
    // and split to both. Remember, a both children have to agree to cull a split.
    if( retVal == kClear )
        return IGetNode(fOuterChild)->ITestSphereRecur(center, rad);

    if( retVal == kCulled )
        return IGetNode(fInnerChild)->ITestSphereRecur(center, rad);

    // Here's the split, to be culled, both children have to
    // say its culled.
    if( kCulled != IGetNode(fOuterChild)->ITestSphereRecur(center, rad) )
        return kSplit;

    if( kCulled != IGetNode(fInnerChild)->ITestSphereRecur(center, rad) )
        return kSplit;

    return kCulled;
}

plCullNode::plCullStatus plCullNode::TestSphere(const hsPoint3& center, hsScalar rad) const
{
    hsScalar dist = fNorm.InnerProduct(center) + fDist;
    if( dist < -rad )
        return kCulled;
    if( dist > rad )
        return kClear;

    return kSplit;
}

// For this Cull Node, recur down the space hierarchy pruning out who to test for the next Cull Node.
plCullNode::plCullStatus plCullNode::ITestNode(const plSpaceTree* space, Int16 who, hsLargeArray<Int16>& clear, hsLargeArray<Int16>& split, hsLargeArray<Int16>& culled) const
{
    if( space->IsDisabled(who) || (space->GetNode(who).fWorldBounds.GetType() != kBoundsNormal) )
    {
        culled.Append(who);
        return kCulled;
    }

    plCullStatus retVal = kClear;
    plCullStatus stat = TestBounds(space->GetNode(who).fWorldBounds);

    switch( stat )
    {
    case kClear:
        clear.Append(who);
        retVal = kClear;
        break;
    case kCulled:
        culled.Append(who);
        retVal = kCulled;
        break;
    case kSplit:
        if( space->GetNode(who).fFlags & plSpaceTreeNode::kIsLeaf )
        {
//          split.Append(who);
            retVal = kPureSplit;
        }
        else
        {
            plCullStatus child0 = ITestNode(space, space->GetNode(who).GetChild(0), clear, split, culled);
            plCullStatus child1 = ITestNode(space, space->GetNode(who).GetChild(1), clear, split, culled);

            if( child0 != child1 )
            {
                if( child0 == kPureSplit )
                    split.Append(space->GetNode(who).GetChild(0));
                else if( child1 == kPureSplit )
                    split.Append(space->GetNode(who).GetChild(1));
                retVal = kSplit;
            }
            else if( child0 == kPureSplit )
            {
                retVal = kPureSplit;
            }
        }
    }
    return retVal;
}

// Cycle through the Cull Nodes, paring down the list of who to test (through ITestNode above).
// We reclaim the scratch indices in clear and split when we're done (SetCount(0)), but we can't
// reclaim the culled, because our caller may be looking at who all we culled. See below in split.
// If a node is disabled, we can just ignore we ever got called.
void plCullNode::ITestNode(const plSpaceTree* space, Int16 who, hsBitVector& totList, hsBitVector& outList) const
{
    if( space->IsDisabled(who) )
        return;

    UInt32 myClearStart = ScratchClear().GetCount();
    UInt32 mySplitStart = ScratchSplit().GetCount();
    UInt32 myCullStart = ScratchCulled().GetCount();

    if( kPureSplit == ITestNode(space, who, ScratchClear(), ScratchSplit(), ScratchCulled()) )
        ScratchSplit().Append(who);

    UInt32 myClearEnd = ScratchClear().GetCount();
    UInt32 mySplitEnd = ScratchSplit().GetCount();
    UInt32 myCullEnd = ScratchCulled().GetCount();

    int i;
    // If there's no OuterChild, everything in clear and split is visible. Everything in culled
    // goes to innerchild (if any).
    if( fOuterChild < 0 )
    {
        plProfile_IncCount(HarvestNodes, myClearEnd - myClearStart + mySplitEnd - mySplitStart);
        // Replace these with a memcopy or something!!!!
        for( i = myClearStart; i < myClearEnd; i++ )
        {
            space->HarvestLeaves(ScratchClear()[i], totList, outList);
        }
        for( i = mySplitStart; i < mySplitEnd; i++ )
        {
            space->HarvestLeaves(ScratchSplit()[i], totList, outList);
        }

        if( fInnerChild >= 0 )
        {
            for( i = myCullStart; i < myCullEnd; i++ )
            {
                IGetNode(fInnerChild)->ITestNode(space, ScratchCulled()[i], totList, outList);
            }
        }
        ScratchClear().SetCount(myClearStart);
        ScratchSplit().SetCount(mySplitStart);
        ScratchCulled().SetCount(myCullStart);

        return;
    }

    // There is an OuterChild, so whether there's an InnerChild or not,
    // everything in ClearList is visible soley on the discretion of OuterChild.
    for( i = myClearStart; i < myClearEnd; i++ )
    {
        IGetNode(fOuterChild)->ITestNode(space, ScratchClear()[i], totList, outList);
    }

    // If there's no InnerChild, then the SplitList is also visible soley
    // on the discretion of OuterChild.
    if( fInnerChild < 0 )
    {
        for( i = mySplitStart; i < mySplitEnd; i++ )
        {
            IGetNode(fOuterChild)->ITestNode(space, ScratchSplit()[i], totList, outList);
        }

        ScratchClear().SetCount(myClearStart);
        ScratchSplit().SetCount(mySplitStart);
        ScratchCulled().SetCount(myCullStart);

        return;
    }

    // There is an inner child. Everything in culled list is visible
    // soley on its discretion.
    for( i = myCullStart; i < myCullEnd; i++ )
    {
        IGetNode(fInnerChild)->ITestNode(space, ScratchCulled()[i], totList, outList);
    }

    // Okay, here's the rub.
    // Everyone in the split list needs to be tested against InnerChild and OuterChild.
    // If either child says it's okay (puts it in OutList), then it's okay.
    // The problem is that if both children say it's okay, it will wind up in outList twice.
    // This is complicated by the fact that outList is still subTrees at this point,
    // so InnerChild adding a subTree and OuterChild adding a child of that subTree isn't
    // even appending the same value to the list.
    // Sooooo.
    // What we do is keep track of every node (interior and leaf) that gets harvested.
    // When we go to harvest a subtree, we check in totList for its bit being set. Bits
    // set in totList are ENTIRE SUBTREE IS HARVESTED. SpaceTree understands this too in
    // its HarvestLeaves. Seems obvious now, but I didn't hear you suggest it.

    for( i = mySplitStart; i < mySplitEnd; i++ )
    {
        IGetNode(fOuterChild)->ITestNode(space, ScratchSplit()[i], totList, outList);
    }

    for( i = mySplitStart; i < mySplitEnd; i++ )
    {
        if( !totList.IsBitSet(ScratchSplit()[i]) )
            IGetNode(fInnerChild)->ITestNode(space, ScratchSplit()[i], totList, outList);
    }

    ScratchClear().SetCount(myClearStart);
    ScratchSplit().SetCount(mySplitStart);
    ScratchCulled().SetCount(myCullStart);
}

void plCullNode::IHarvest(const plSpaceTree* space, hsTArray<Int16>& outList) const
{
    ITestNode(space, space->GetRoot(), ScratchTotVec(), ScratchBitVec());
    space->BitVectorToList(outList, ScratchBitVec());
    ScratchBitVec().Clear();
    ScratchTotVec().Clear();

    ScratchClear().SetCount(0);
    ScratchSplit().SetCount(0);
    ScratchCulled().SetCount(0);
}

//////////////////////////////////////////////////////////////////////
// This section builds the tree from the input cullpoly's
//////////////////////////////////////////////////////////////////////

void plCullNode::IBreakPoly(const plCullPoly& poly, const hsTArray<hsScalar>& depths,
                            hsBitVector& inVerts,
                            hsBitVector& outVerts,
                            hsBitVector& onVerts,
                            plCullPoly& outPoly) const
{
    inVerts.Clear();
    outVerts.Clear();
    onVerts.Clear();

    outPoly.Init(poly);

    if( depths[0] < -kTolerance )
        inVerts.SetBit(0);
    else if( depths[0] > kTolerance )
        outVerts.SetBit(0);
    else
        onVerts.SetBit(0);
    
    if( poly.fClipped.IsBitSet(0) )
        outPoly.fClipped.SetBit(0);
    outPoly.fVerts.Append(poly.fVerts[0]);

    int i;
    for( i = 1; i < poly.fVerts.GetCount(); i++ )
    {
        if( depths[i] < -kTolerance )
        {
            if( outVerts.IsBitSet(outPoly.fVerts.GetCount()-1) )
            {
                hsPoint3 interp;
                hsScalar t = IInterpVert(poly.fVerts[i-1], poly.fVerts[i], interp);
                // add interp
                onVerts.SetBit(outPoly.fVerts.GetCount());
                if( poly.fClipped.IsBitSet(i-1) )
                    outPoly.fClipped.SetBit(outPoly.fVerts.GetCount());
                outPoly.fVerts.Append(interp);
            }
            inVerts.SetBit(outPoly.fVerts.GetCount());
        }
        else if( depths[i] > kTolerance )
        {
            if( inVerts.IsBitSet(outPoly.fVerts.GetCount()-1) )
            {
                hsPoint3 interp;
                hsScalar t = IInterpVert(poly.fVerts[i-1], poly.fVerts[i], interp);
                // add interp
                onVerts.SetBit(outPoly.fVerts.GetCount());
                if( poly.fClipped.IsBitSet(i-1) )
                    outPoly.fClipped.SetBit(outPoly.fVerts.GetCount());
                outPoly.fVerts.Append(interp);
            }
            outVerts.SetBit(outPoly.fVerts.GetCount());
        }
        else
        {
            onVerts.SetBit(outPoly.fVerts.GetCount());
        }

        if( poly.fClipped.IsBitSet(i) )
            outPoly.fClipped.SetBit(outPoly.fVerts.GetCount());
        outPoly.fVerts.Append(poly.fVerts[i]);
    }
    if( (inVerts.IsBitSet(outPoly.fVerts.GetCount()-1) && outVerts.IsBitSet(0))
        ||(outVerts.IsBitSet(outPoly.fVerts.GetCount()-1) && inVerts.IsBitSet(0)) )
    {
        hsPoint3 interp;
        hsScalar t = IInterpVert(poly.fVerts[poly.fVerts.GetCount()-1], poly.fVerts[0], interp);
        onVerts.SetBit(outPoly.fVerts.GetCount());
        if( poly.fClipped.IsBitSet(poly.fVerts.GetCount()-1) )
            outPoly.fClipped.SetBit(outPoly.fVerts.GetCount());
        outPoly.fVerts.Append(interp);
    }
}

void plCullNode::ITakeHalfPoly(const plCullPoly& srcPoly, 
                               const hsTArray<int>& vtxIdx, 
                               const hsBitVector& onVerts, 
                               plCullPoly& outPoly) const
{
    if( vtxIdx.GetCount() > 2 )
    {
        int i;
        for( i = 0; i < vtxIdx.GetCount(); i++ )
        {
            int next = i < vtxIdx.GetCount()-1 ? i+1 : 0;
            int last = i ? i-1 : vtxIdx.GetCount()-1;

            // If these 3 verts are all on the plane, we may have created a collinear vertex (the middle one)
            // which we now want to skip.
            if( onVerts.IsBitSet(vtxIdx[i]) && onVerts.IsBitSet(vtxIdx[last]) && onVerts.IsBitSet(vtxIdx[next]) )
            {
#if 0 // FISH
                hsScalar dot = hsVector3(&srcPoly.fVerts[vtxIdx[last]], &srcPoly.fVerts[vtxIdx[i]]).InnerProduct(hsVector3(&srcPoly.fVerts[vtxIdx[next]], &srcPoly.fVerts[vtxIdx[i]]));
                if( dot <= 0 )
#endif // FISH
                    continue;
            }
            if( srcPoly.fClipped.IsBitSet(vtxIdx[i])
                ||(onVerts.IsBitSet(vtxIdx[i]) && onVerts.IsBitSet(vtxIdx[next])) )
                    outPoly.fClipped.SetBit(outPoly.fVerts.GetCount());
            outPoly.fVerts.Append(srcPoly.fVerts[vtxIdx[i]]);
        }
    }
    else
    {
        // Just need a break point
        hsStatusMessage("Under 2"); // FISH
    }
}

void plCullNode::IMarkClipped(const plCullPoly& poly, const hsBitVector& onVerts) const
{
    int i;
    for( i = 1; i < poly.fVerts.GetCount(); i++ )
    {
        int last = i-1;
        if( onVerts[i] && onVerts[last] )
            poly.fClipped.SetBit(last);
    }
    if( onVerts[i] && onVerts[0] )
        poly.fClipped.SetBit(0);
}

plCullNode::plCullStatus plCullNode::ISplitPoly(const plCullPoly& poly, 
                                                plCullPoly*& innerPoly, 
                                                plCullPoly*& outerPoly) const
{
    static hsTArray<hsScalar> depths;
    depths.SetCount(poly.fVerts.GetCount());

    static hsBitVector onVerts;
    onVerts.Clear();

    hsBool someInner = false;
    hsBool someOuter = false;
    hsBool someOn = false;
    int i;
    for( i = 0; i < poly.fVerts.GetCount(); i++ )
    {
        depths[i] = fNorm.InnerProduct(poly.fVerts[i]) + fDist;
        if( depths[i] < -kTolerance )
            someInner = true;
        else if( depths[i] > kTolerance )
            someOuter = true;
        else 
        {
            someOn = true;
            onVerts.SetBit(i);
        }
    }
    if( !(someInner || someOuter) )
    {
        (innerPoly = ScratchPolys().Push())->Init(poly);
        (outerPoly = ScratchPolys().Push())->Init(poly);
        return kSplit;
    }
    else if( !someInner )
    {
        IMarkClipped(poly, onVerts);
        return kClear;
    }
    else if( !someOuter )
    {
        IMarkClipped(poly, onVerts);
        return kCulled;
    }


    // Okay, it's split, now break it into the two polys
    (innerPoly = ScratchPolys().Push())->Init(poly);
    (outerPoly = ScratchPolys().Push())->Init(poly);

    static plCullPoly scrPoly;

    static hsBitVector inVerts;
    static hsBitVector outVerts;

    IBreakPoly(poly, depths,
        inVerts,
        outVerts,
        onVerts,
        scrPoly);

    static hsTArray<int> inPolyIdx;
    inPolyIdx.SetCount(0);
    static hsTArray<int> outPolyIdx;
    outPolyIdx.SetCount(0);

    for( i = 0; i < scrPoly.fVerts.GetCount(); i++ )
    {
        if( inVerts.IsBitSet(i) )
        {
            inPolyIdx.Append(i);
        }
        else if( outVerts.IsBitSet(i) )
        {
            outPolyIdx.Append(i);
        }
        else
        {
            inPolyIdx.Append(i);
            outPolyIdx.Append(i);
        }
    }

    ITakeHalfPoly(scrPoly, inPolyIdx, onVerts, *innerPoly);

    ITakeHalfPoly(scrPoly, outPolyIdx, onVerts, *outerPoly);

    return kSplit;
}

hsScalar plCullNode::IInterpVert(const hsPoint3& p0, const hsPoint3& p1, hsPoint3& out) const
{
    hsVector3 oneToOh;
    oneToOh.Set(&p0, &p1);

    hsScalar t = -(fNorm.InnerProduct(p1) + fDist) / fNorm.InnerProduct(oneToOh);
    if( t >= 1.f )
    {
        out = p0;
        return 1.f;
    }
    if( t <= 0 )
    {
        out = p1;
        return 0;
    }

    out = p1;

    out += oneToOh * t;

    return t;
}

// We use indices so our tree can actually be an array, which may be
// resized at any time, which would invalidate any pointers we have.
// But debugging a large tree is hard enough when stepping through pointers,
// it's pretty much impossible with indices. So when debugging we can 
// setup these pointers for stepping through the tree. We just need to
// reset them every time we add a poly, because that's when the tree
// may have been resized invalidating the old pointers.
#ifdef DEBUG_POINTERS
void plCullNode::ISetPointersRecur() const
{
    if( fInnerPtr = IGetNode(fInnerChild) )
        fInnerPtr->ISetPointersRecur();
    if( fOuterPtr = IGetNode(fOuterChild) )
        fOuterPtr->ISetPointersRecur();
}
#endif // DEBUG_POINTERS

//////////////////////////////////////////////////////////////////////
// Now the tree proper
//////////////////////////////////////////////////////////////////////
// Build the tree
plCullTree::plCullTree()
:   fRoot(-1),
    fCapturePolys(false)
{
}

plCullTree::~plCullTree()
{
}

void plCullTree::AddPoly(const plCullPoly& poly)
{
    const plCullPoly* usePoly = &poly;

    hsVector3 cenToEye(&fViewPos, &poly.fCenter);
    hsFastMath::NormalizeAppr(cenToEye);
    hsScalar camDist = cenToEye.InnerProduct(poly.fNorm);
    plConst(hsScalar) kTol(0.1f);
    hsBool backFace = camDist < -kTol;
    if( !backFace && (camDist < kTol) )
        return;

    plCullPoly scratchPoly;
    if( poly.IsHole() )
    {
        if( !backFace )
            return;
    }
    else
    if( backFace )
    {
        plConst(hsBool) kAllowTwoSided(true);
        if( !kAllowTwoSided || !poly.IsTwoSided() )
            return;

        scratchPoly.Flip(poly);
        usePoly = &scratchPoly;
    }

    if( !SphereVisible(usePoly->GetCenter(), usePoly->GetRadius()) )
        return;

    usePoly->fClipped.Clear();

    usePoly->Validate();

    // Make sure we have enough scratch polys. Each node
    // can potentially split this poly, so...
    ISetupScratch(fNodeList.GetCount());

#if 1
    if( IGetRoot() && IGetNode(IGetRoot()->fOuterChild) )
    {
        IAddPolyRecur(*usePoly, IGetRoot()->fOuterChild);
    }
    else
#endif
    {
        fRoot = IAddPolyRecur(*usePoly, fRoot);
    }

#ifdef DEBUG_POINTERS
    if( IGetRoot() )
        IGetRoot()->ISetPointersRecur();
#endif // DEBUG_POINTERS
}

Int16 plCullTree::IAddPolyRecur(const plCullPoly& poly, Int16 iNode)
{
    if( poly.fVerts.GetCount() < 3 )
        return iNode;

    if( iNode < 0 )
        return IMakePolySubTree(poly);

    hsBool addInner = (IGetNode(iNode)->fInnerChild >= 0)
                        || ((iNode > 5) && poly.IsHole());
    hsBool addOuter = !poly.IsHole() || (IGetNode(iNode)->fOuterChild >= 0);

    plCullPoly* innerPoly = nil;
    plCullPoly* outerPoly = nil;

    plCullNode::plCullStatus test = IGetNode(iNode)->ISplitPoly(poly, innerPoly, outerPoly);

    switch( test )
    {
    case plCullNode::kClear:
        if( addOuter )
        {
            int child = IAddPolyRecur(poly, IGetNode(iNode)->fOuterChild);
            IGetNode(iNode)->fOuterChild = child;
        }
        break;
    case plCullNode::kCulled:
        if( addInner )
        {
            int child = IAddPolyRecur(poly, IGetNode(iNode)->fInnerChild);
            IGetNode(iNode)->fInnerChild = child;
        }
        break;
    case plCullNode::kSplit:
        hsAssert(innerPoly && outerPoly, "Poly should have been split into inner and outer in SplitPoly");
        if( addOuter )
        {
            int child = IAddPolyRecur(*outerPoly, IGetNode(iNode)->fOuterChild);
            IGetNode(iNode)->fOuterChild = child;
        }
        if( addInner )
        {
            int child = IAddPolyRecur(*innerPoly, IGetNode(iNode)->fInnerChild);
            IGetNode(iNode)->fInnerChild = child;
        }
        break;
    }
    return iNode;
}

Int16 plCullTree::IMakePolyNode(const plCullPoly& poly, int i0, int i1) const
{
    Int16 retINode = fNodeList.GetCount();
    plCullNode* nextNode = fNodeList.Push();
    hsVector3 a;
    hsVector3 b;
    a.Set(&poly.fVerts[i0], &fViewPos);
    b.Set(&poly.fVerts[i1], &fViewPos);
    hsVector3 n = a % b;
    hsScalar d = -n.InnerProduct(fViewPos);

    IDEBUG_NORMALIZE(n, d);

    nextNode->Init(this, n, d);

    return retINode;
}

Int16 plCullTree::IMakeHoleSubTree(const plCullPoly& poly) const
{
    if( fCapturePolys )
        IVisPoly(poly, true);

    int firstNode = fNodeList.GetCount();

    Int16 iNode = -1;

    int i;
    for( i = 0; i < poly.fVerts.GetCount()-1; i++ )
    {
        if( !poly.fClipped.IsBitSet(i) )
        {
            Int16 child = IMakePolyNode(poly, i, i+1);
            if( iNode >= 0 )
                IGetNode(iNode)->fOuterChild = child;
            iNode = child;
        }
    }
    if( !poly.fClipped.IsBitSet(i) )
    {
        Int16 child = IMakePolyNode(poly, i, 0);
        if( iNode >= 0 )
            IGetNode(iNode)->fOuterChild = child;
        iNode = child;
    }

    plCullNode* child = fNodeList.Push();
    child->Init(this, poly.fNorm, poly.fDist);
    if( iNode >= 0 )
        IGetNode(iNode)->fOuterChild = fNodeList.GetCount()-1;

    return firstNode;
}

Int16 plCullTree::IMakePolySubTree(const plCullPoly& poly) const
{
    poly.Validate();

    if( poly.IsHole() )
        return IMakeHoleSubTree(poly);

    if( fCapturePolys )
        IVisPoly(poly, false);

    int firstNode = fNodeList.GetCount();

    Int16 iNode = -1;

    int i;
    for( i = 0; i < poly.fVerts.GetCount()-1; i++ )
    {
        if( !poly.fClipped.IsBitSet(i) )
        {
            Int16 child = IMakePolyNode(poly, i, i+1);
            if( iNode >= 0 )
                IGetNode(iNode)->fInnerChild = child;
            iNode = child;
        }
    }
    if( !poly.fClipped.IsBitSet(i) )
    {
        Int16 child = IMakePolyNode(poly, i, 0);
        if( iNode >= 0 )
            IGetNode(iNode)->fInnerChild = child;
        iNode = child;
    }

    plCullNode* child = fNodeList.Push();
    child->Init(this, poly.fNorm, poly.fDist);
    child->fIsFace = true;
    if( iNode >= 0 )
        IGetNode(iNode)->fInnerChild = fNodeList.GetCount()-1;

    return firstNode;
}

///////////////////////////////////////////////////////////////////
// Begin visualization section of the program
///////////////////////////////////////////////////////////////////
void plCullTree::IVisPolyShape(const plCullPoly& poly, hsBool dark) const
{
    int i;

    int vertStart = fVisVerts.GetCount();
    
    hsColorRGBA color;
    if( dark )
        color.Set(0.2f, 0.2f, 0.2f, 1.f);
    else
        color.Set(1.f, 1.f, 1.f, 1.f);

    hsVector3 norm = dark ? -poly.fNorm : poly.fNorm;

    for( i = 0; i < poly.fVerts.GetCount(); i++ )
    {
        fVisVerts.Append(poly.fVerts[i]);
        fVisNorms.Append(poly.fNorm);
        fVisColors.Append(color);
    }
    if( !dark )
    {
        for( i = 2; i < poly.fVerts.GetCount(); i++ )
        {
            fVisTris.Append(vertStart);
            fVisTris.Append(vertStart + i-1);
            fVisTris.Append(vertStart + i);
        }
    }
    else
    {
        for( i = 2; i < poly.fVerts.GetCount(); i++ )
        {
            fVisTris.Append(vertStart);
            fVisTris.Append(vertStart + i);
            fVisTris.Append(vertStart + i-1);
        }
    }
}

void plCullTree::IVisPolyEdge(const hsPoint3& p0, const hsPoint3& p1, hsBool dark) const
{
    hsColorRGBA color;
    if( dark )
        color.Set(0.2f, 0.2f, 0.2f, 1.f);
    else
        color.Set(1.f, 1.f, 1.f, 1.f);

    int vertStart = fVisVerts.GetCount();

    hsVector3 dir0(&p0, &fViewPos);
    hsFastMath::NormalizeAppr(dir0);
    dir0 *= fVisYon;
    hsVector3 dir1(&p1, &fViewPos);
    hsFastMath::NormalizeAppr(dir1);
    dir1 *= fVisYon;

    hsPoint3 p3 = fViewPos;
    p3 += dir0;
    hsPoint3 p2 = fViewPos;
    p2 += dir1;

    hsVector3 norm = hsVector3(&p0, &fViewPos) % hsVector3(&p1, &fViewPos);
    hsFastMath::NormalizeAppr(norm);

    fVisVerts.Append(p0);
    fVisNorms.Append(norm);
    fVisColors.Append(color);
    fVisVerts.Append(p1);
    fVisNorms.Append(norm);
    fVisColors.Append(color);
    fVisVerts.Append(p2);
    fVisNorms.Append(norm);
    fVisColors.Append(color);
    fVisVerts.Append(p3);
    fVisNorms.Append(norm);
    fVisColors.Append(color);

    fVisTris.Append(vertStart + 0);
    fVisTris.Append(vertStart + 2);
    fVisTris.Append(vertStart + 1);

    fVisTris.Append(vertStart + 0);
    fVisTris.Append(vertStart + 3);
    fVisTris.Append(vertStart + 2);
}

void plCullTree::IVisPoly(const plCullPoly& poly, hsBool dark) const
{
    IVisPolyShape(poly, dark);

    int i;
    for( i = 0; i < poly.fVerts.GetCount()-1; i++ )
    {
        if( !poly.fClipped.IsBitSet(i) )
            IVisPolyEdge(poly.fVerts[i], poly.fVerts[i+1], dark);
    }
    if( !poly.fClipped.IsBitSet(i) )
        IVisPolyEdge(poly.fVerts[i], poly.fVerts[0], dark);
}

void plCullTree::ReleaseCapture() const
{
    fVisVerts.Reset();
    fVisNorms.Reset();
    fVisColors.Reset();
    fVisTris.Reset();
}

///////////////////////////////////////////////////////////////////
// End visualization section of the program
///////////////////////////////////////////////////////////////////

void plCullTree::ISetupScratch(UInt16 nNodes)
{
    ScratchPolys().SetCount(nNodes << 1);
    ScratchPolys().SetCount(0);
}

void plCullTree::Reset()
{
    // Using NodeList as scratch will only work if we use indices,
    // because a push invalidates any pointers we've stored away.
    fNodeList.SetCount(0);

    fRoot = -1;

    ScratchPolys().SetCount(0);
}


void plCullTree::InitFrustum(const hsMatrix44& world2NDC)
{
    Reset();

    fNodeList.SetCount(6);
    fNodeList.SetCount(0);

    Int16       lastIdx = -1;

    plCullNode* node;
    hsVector3 norm;
    hsScalar dist;

    int i;
    for( i = 0; i < 2; i++ )
    {
        
        norm.Set(world2NDC.fMap[3][0] - world2NDC.fMap[i][0], world2NDC.fMap[3][1] - world2NDC.fMap[i][1], world2NDC.fMap[3][2] - world2NDC.fMap[i][2]);
        dist = world2NDC.fMap[3][3] - world2NDC.fMap[i][3];

        IDEBUG_NORMALIZE( norm, dist );

        node = fNodeList.Push();
        node->Init(this, norm, dist);
        node->fOuterChild = lastIdx;
        lastIdx = fNodeList.GetCount()-1;

        norm.Set(world2NDC.fMap[3][0] + world2NDC.fMap[i][0], world2NDC.fMap[3][1] + world2NDC.fMap[i][1], world2NDC.fMap[3][2] + world2NDC.fMap[i][2]);
        dist = world2NDC.fMap[3][3] + world2NDC.fMap[i][3];

        IDEBUG_NORMALIZE( norm, dist );

        node = fNodeList.Push();
        node->Init(this, norm, dist);
        node->fOuterChild = lastIdx;
        lastIdx = fNodeList.GetCount()-1;
    }
    norm.Set(world2NDC.fMap[3][0] - world2NDC.fMap[2][0], world2NDC.fMap[3][1] - world2NDC.fMap[2][1], world2NDC.fMap[3][2] - world2NDC.fMap[2][2]);
    dist = world2NDC.fMap[3][3] - world2NDC.fMap[2][3];

    IDEBUG_NORMALIZE( norm, dist );

    node = fNodeList.Push();
    node->Init(this, norm, dist);
    node->fOuterChild = lastIdx;
    lastIdx = fNodeList.GetCount()-1;

#ifdef SYMMET
    norm.Set(world2NDC.fMap[3][0] + world2NDC.fMap[2][0], world2NDC.fMap[3][1] + world2NDC.fMap[2][1], world2NDC.fMap[3][2] + world2NDC.fMap[2][2]);
    dist = world2NDC.fMap[3][3] + world2NDC.fMap[2][3];
#else // SYMMET
    norm.Set(world2NDC.fMap[2][0], world2NDC.fMap[2][1], world2NDC.fMap[2][2]);
    dist = world2NDC.fMap[2][3];
#endif // SYMMET

    IDEBUG_NORMALIZE( norm, dist );

    node = fNodeList.Push();
    node->Init(this, norm, dist);
    node->fOuterChild = lastIdx;
    lastIdx = fNodeList.GetCount()-1;

    fRoot = fNodeList.GetCount()-1;

#ifdef DEBUG_POINTERS
    if( IGetRoot() )
        IGetRoot()->ISetPointersRecur();
#endif // DEBUG_POINTERS
}

void plCullTree::SetViewPos(const hsPoint3& p)
{
    fViewPos = p;
}

//////////////////////////////////////////////////////////////////////
// Use the tree
//////////////////////////////////////////////////////////////////////
void plCullTree::Harvest(const plSpaceTree* space, hsTArray<Int16>& outList) const
{
    outList.SetCount(0);
    if (!space->IsEmpty())
        IGetRoot()->IHarvest(space, outList);

}

hsBool plCullTree::BoundsVisible(const hsBounds3Ext& bnd) const
{
    return plCullNode::kCulled != IGetRoot()->ITestBoundsRecur(bnd);
}

hsBool plCullTree::SphereVisible(const hsPoint3& center, hsScalar rad) const
{
    return plCullNode::kCulled != IGetRoot()->ITestSphereRecur(center, rad);
}