/*==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/>.

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