/*
 * Collision plugin for Renderware.
 *
 */

/*
 *  File : ctbuild.c
 *
 *  This section deals entirely with building a raw collision BSP tree
 *  from a list of vertices and polygons, and is likely to become part
 *  of a toolkit.
 */

/******************************************************************************
 *  Include files
 */

#include <rwcore.h>
#include "rpplugin.h"
#include <rpdbgerr.h>

#include "ctbsp.h"
#include "ctbuild.h"

#if (!defined(DOXYGEN))
static const char   rcsid[] __RWUNUSED__ =
    "@@@@(#)$Id: ctbuild.c,v 1.23 2001/07/05 08:28:07 johns Exp $";
#endif /* (!defined(DOXYGEN)) */

/******************************************************************************
 *  Defines
 */

#define CLIPVERTEXLEFT      (1)
#define CLIPVERTEXRIGHT     (2)
#define CLIPPOLYGONLEFT      (1)
#define CLIPPOLYGONRIGHT     (2)

/******************************************************************************
 *  Types
 */

/* Following types used just for build process */

typedef struct collBSPSector collBSPSector;
struct collBSPSector
{
    RwInt32             type;   /* = -1 for poly sector, >=0 for plane sector */
};

typedef struct collBSPPolygonSector collBSPPolygonSector;
struct collBSPPolygonSector
{
    RwInt32             type;   /* = -1 for poly sector */
    RwUInt16            numPolygons; /* num polys in sector */
    RwUInt16            iFirstPolygon; /* index of first poly in polyMap[] */
};

typedef struct collBSPPlaneSector collBSPPlaneSector;
struct collBSPPlaneSector
{
    RwInt32             type;   /* plane type */
    collBSPSector      *leftSubTree;
    collBSPSector      *rightSubTree;
    RwReal              leftValue;
    RwReal              rightValue;
};

typedef struct buildVertex buildVertex;
struct buildVertex
{
    RwV3d              *pos;
    RwUInt32            flags;
    RwReal              dist;
};

typedef struct buildPolygon buildPolygon;
struct buildPolygon
{
    RpCollBSPTriangle  *poly;
    RwUInt32            flags;
};

typedef struct buildData buildData;
struct buildData
{
    /* Data for whole tree */
    RwUInt32            numVertices;
    buildVertex        *vertices;
    buildPolygon       *polygons; /* Sorted on completion */

    /* Data for current sector in build process */
    RwUInt32            bspDepth;
    RwBBox              bbox;
    RwUInt32            numPolygons;
    RwUInt32            polygonOffset; /* Offset into polygons[] */

    /* Pointer array for sorting the subset of vertices for polygons in 
     * current sector */
    RwUInt32            numSortVerts;
    buildVertex       **sortVerts;
};

typedef struct clipStatistics clipStatistics;
struct clipStatistics
{
    RwInt32             nLeft;
    RwInt32             nRight;
    RwReal              leftValue;
    RwReal              rightValue;
};

/******************************************************************************
 *  Macros
 */

#define SetClipCodes(numVert, sortVerts, plane, value)                   \
MACRO_START                                                              \
{                                                                        \
    RwUInt32     iVert;                                                  \
                                                                         \
    for (iVert=0; iVert<numVert; iVert++)                                \
    {                                                                    \
        RwReal test = GETCOORD(*(sortVerts[iVert]->pos), plane) - value; \
                                                                         \
        sortVerts[iVert]->flags = 0;                                     \
                                                                         \
        /*                                                               \
         *  NOTE: if the vertex is within the plane it                   \
         *  will be classifed as being on both sides of the plane,       \
         *  this is intentional.                                         \
         */                                                              \
        if (test <= ((RwReal)0))                                         \
        {                                                                \
            sortVerts[iVert]->flags |= CLIPVERTEXLEFT;                   \
        }                                                                \
                                                                         \
        if (test >= ((RwReal)0))                                         \
        {                                                                \
            sortVerts[iVert]->flags |= CLIPVERTEXRIGHT;                  \
        }                                                                \
    }                                                                    \
}                                                                        \
MACRO_STOP

/******************************************************************************
 *
 *  Get clip statistics.
 *
 *  Return stats:

    stats->leftValue        extent of left sector
    stats->rightValue       extent of right sector
    stats->nLeft            number of polys on left
    stats->nRight           number of polys on right
 */

static void
GetClipStatistics(buildData * data,
                  RwInt32 plane, RwReal value, clipStatistics * stats)
{
    RwReal              overlapLeft;
    RwReal              overlapRight;
    RwInt32             numPolygons;
    buildPolygon       *polygons;
    const buildVertex  *vertices;
    RwInt32             nLeft = 0;
    RwInt32             nRight = 0;

    RWFUNCTION(RWSTRING("GetClipStatistics"));
    RWASSERT(data);
    RWASSERT(stats);

    polygons = &data->polygons[data->polygonOffset];
    vertices = data->vertices;

    nLeft = nRight = 0;
    overlapLeft = overlapRight = (RwReal) 0;

    /* For each polygon */
    for (numPolygons = data->numPolygons; --numPolygons >=0; polygons++)
    {
        const RwUInt16     *const vertIndex = polygons->poly->vertIndex;
        RwUInt32            clip, iTriVert;
        RwReal              distLeft, distRight;

        clip = CLIPVERTEXLEFT | CLIPVERTEXRIGHT;
        distLeft = distRight = ((RwReal) 0);

        /* For each vertex */
        for (iTriVert = 0; iTriVert < 3; iTriVert++)
        {
            const RwUInt32      iVert = vertIndex[iTriVert];
            RwV3d * const       pos = vertices[iVert].pos;
            const RwReal        dist = GETCOORD(*pos, plane) - value;
            const RwReal        mdist = -dist;

            /* Mask clip flags for polygon */
            clip &= vertices[iVert].flags;

            /* Extent of poly either side of the plane */
            if (mdist > distLeft)
            {
                distLeft = mdist;
            }
            else if (dist > distRight)
            {
                distRight = dist;
            }
        }

        /* figure stats for this polygon */
        switch (clip & (CLIPVERTEXLEFT | CLIPVERTEXRIGHT))
        {
            case CLIPVERTEXLEFT:

                /* Poly is entirely on the left */
                polygons->flags = CLIPPOLYGONLEFT;
                nLeft++;
                break;

            case CLIPVERTEXLEFT | CLIPVERTEXRIGHT:
            case CLIPVERTEXRIGHT:

                /* Poly is entirely on the right */
                polygons->flags = CLIPPOLYGONRIGHT;
                nRight++;
                break;

            case 0:

                /* Decide which overlapping sector poly belongs to */
                if (distRight > distLeft)
                {
                    nRight++;
                    polygons->flags = CLIPPOLYGONRIGHT;
                    if (distLeft > overlapRight)
                    {
                        overlapRight = distLeft;
                    }
                }
                else
                {
                    nLeft++;
                    polygons->flags = CLIPPOLYGONLEFT;
                    if (distRight > overlapLeft)
                    {
                        overlapLeft = distRight;
                    }
                }
                break;
        }
    }

    /* All done */
    stats->nLeft = nLeft;
    stats->nRight = nRight;
    stats->leftValue = value + overlapLeft;
    stats->rightValue = value - overlapRight;

    RWRETURNVOID();
}

/******************************************************************************
 *
 *  Calculate average depth of leaf nodes in a binary tree, assuming that
 *  all nodes have equal lookup probability, and the tree is as symmetric as
 *  possible.
 *
 *  In the case of choosing BSP sectors, this assumption gives maximum 
 *  significance to the choice we are making at the current level of
 *  building the tree.
 */
static              RwReal
AverageTreeDepth(RwUInt32 numLeaves)
{
    RwReal              aveDepth;
    RwInt32             depth;
    RwUInt32            temp;

    RWFUNCTION(RWSTRING("AverageTreeDepth"));
    RWASSERT(numLeaves);

    /* Depth is MSB */
    depth = -1;
    temp = numLeaves;
    while (temp)
    {
        depth++;
        temp >>= 1;
    }

    /* Average over all leaf nodes */
    aveDepth =
        (RwReal) (depth + 2) - (RwReal) (1 << (depth + 1)) / numLeaves;

    RWRETURN(aveDepth);
}

/******************************************************************************
 *
 *  Score the division of the sector for current plane and value,
 *  based on the clip statistics.
 */
static              RwReal
DivisionScore(buildData * data, clipStatistics * stats, RwInt32 plane)
{
    RwReal              leftExtent, rightExtent, sectorExtent;
    RwReal              score;

    RWFUNCTION(RWSTRING("DivisionScore"));

    /*  These factors are proportional to the volumes of the left and 
     *  right sectors, so that we effectively weight the scores for 
     *  subsectors by volume, which makes sense for point-like collision
     *  tests. This also weights fairly against overlap.
     */
    sectorExtent = GETCOORD(data->bbox.sup, plane)
        - GETCOORD(data->bbox.inf, plane);
    leftExtent = stats->leftValue - GETCOORD(data->bbox.inf, plane);
    rightExtent = GETCOORD(data->bbox.sup, plane) - stats->rightValue;

    score = ((leftExtent * AverageTreeDepth(stats->nLeft) +
              rightExtent * AverageTreeDepth(stats->nRight)) /
             sectorExtent);

    RWRETURN(score);
}

/******************************************************************************
 *
 * Create list of pointers sortVerts[numSortVerts] to all vertices 
 * referenced by polygons in data. This will, in general, be a subset
 * of vertices[numVertices].
 */
static void
SetSortVertices(buildData * data)
{
    RwUInt32            iPoly;
    RwUInt32            iVert;
    RwUInt32            iSort;
    buildPolygon       *polygons;

    RWFUNCTION(RWSTRING("SetSortVertices"));

    polygons = data->polygons + data->polygonOffset;

    /* Reset flags for all vertices */
    for (iVert = 0; iVert < data->numVertices; iVert++)
    {
        data->vertices[iVert].flags = 0;
    }

    /* For each polygon */
    for (iPoly = 0; iPoly < data->numPolygons; iPoly++)
    {
        RwInt32             iPolyVert;

        /* For each polygon vertex */
        for (iPolyVert = 0; iPolyVert < 3; iPolyVert++)
        {
            /* Flag as required */
            data->vertices[polygons[iPoly].poly->vertIndex[iPolyVert]].
                flags++;
        }
    }

    /* Create a sort vertex pointer for each vertex required */
    iSort = 0;
    for (iVert = 0; iVert < data->numVertices; iVert++)
    {
        if (data->vertices[iVert].flags)
        {
            data->sortVerts[iSort] = &data->vertices[iVert];
            iSort++;
        }
    }

    data->numSortVerts = iSort;

    RWRETURNVOID();
}

/******************************************************************************
 *
 *  Weight against splitting of sectors in smallest dimension. It is more
 *  efficient to have cube shaped sectors than slabs when testing lines.
 */
static              RwReal
FuzzyExtentScore(RwReal extent, RwReal maxExtent)
{
    RwReal              value;

    RWFUNCTION(RWSTRING("FuzzyExtentScore"));

    value = extent / maxExtent;
    if (value > ((RwReal) 0.5))
    {
        value = ((RwReal) 1);
    }
    else
    {
        /* bias against cutting small extent */
        value = ((RwReal) 0.5) + value;
    }

    value = (RwReal) 1 / value;
    RWRETURN(value);
}

/*****************************************************************************
 *
 *  Find the best plane that splits the current sets of polygons into two.
 *  This is based on the world sector cut plane algorithm.
 */

#define MAXCLOSESTCHECK 50
#define RANGEEPS 0.0001f

static int
buildVertexCmp(const void *a, const void *b)
{
    volatile RwSplitBits diff;
    int                 result;

    RWFUNCTION(RWSTRING("buildVertexCmp"));
    RWASSERT(a);
    RWASSERT(b);

    diff.nReal = ((*((const buildVertex **) a))->dist -
                  (*((const buildVertex **) b))->dist);
    result = diff.nInt;

    RWRETURN(result);
}

static              RwBool
FindDividingPlane(buildData * data, RwInt32 * plane, RwReal * value)
{
    RwReal              bestScore = RwRealMAXVAL; /* Haven't found any */
    RwInt32             bestPlane = (RwInt32) 0;
    RwReal              bestValue = (RwReal) 0;
    clipStatistics      bestStats;
    RwInt32             tryPlane, maxExtentPlane;
    RwV3d               vSize;

    RWFUNCTION(RWSTRING("FindDividingPlane"));

    /* Find the maximum extent for this sector */
    RwV3dSub(&vSize, &data->bbox.sup, &data->bbox.inf);

    maxExtentPlane = 0;
    for (tryPlane = 0; tryPlane < 12; tryPlane += sizeof(RwReal))
    {
        if (GETCOORD(vSize, tryPlane) > GETCOORD(vSize, maxExtentPlane))
        {
            maxExtentPlane = tryPlane;
        }
    }

    /* Go through the planes -> try and find the best one */
    for (tryPlane = 0;
         tryPlane < (RwInt32) (3 * sizeof(RwReal));
         tryPlane += sizeof(RwReal))
    {
        RwReal              quantThresh;
        RwReal              median;
        RwReal              interQuartileRange;
        RwInt32             lastquantValue;
        RwUInt32            lowerQuartile;
        RwUInt32            upperQuartile;
        RwInt32             numUniq;
        RwUInt32            nI;
        RwReal              extentScore;

        extentScore = FuzzyExtentScore(GETCOORD(vSize, tryPlane),
                                       GETCOORD(vSize, maxExtentPlane));

        /* find median of population */
        for (nI = 0; nI < data->numSortVerts; nI++)
        {
            data->sortVerts[nI]->dist =
                GETCOORD(*(data->sortVerts[nI]->pos), tryPlane);
        }
        qsort((void *) data->sortVerts, data->numSortVerts,
              sizeof(buildVertex *), buildVertexCmp);

        /* value that bisects population in tryPlane axis */
        median = data->sortVerts[nI >> 1]->dist;
        lowerQuartile = nI >> 2; /* 25% */
        upperQuartile = (nI >> 1) + (nI >> 2); /* 75% */

        /* granularity by which we move plane around +/-25% of median */
        interQuartileRange = data->sortVerts[upperQuartile]->dist -
            data->sortVerts[lowerQuartile]->dist;

        /* Is inter-quartile range very small? */
        if (interQuartileRange <
            (RANGEEPS * GETCOORD(vSize, maxExtentPlane)))
        {
            /* Just test median value for split */
            lowerQuartile = upperQuartile = (nI >> 1);
            quantThresh = ((RwReal) 0);
        }
        else
        {
            quantThresh = MAXCLOSESTCHECK / interQuartileRange;

            /* sort just this subarray on abs dist from median */
            for (nI = lowerQuartile; nI <= upperQuartile; nI++)
            {
                RwReal              dist =
                    median - data->sortVerts[nI]->dist;

                if (dist < ((RwReal) 0))
                {
                    data->sortVerts[nI]->dist = -dist;
                }
                else
                {
                    data->sortVerts[nI]->dist = dist;
                }
            }
            qsort((void *) &data->sortVerts[lowerQuartile],
                  upperQuartile - lowerQuartile + 1,
                  sizeof(buildVertex *), buildVertexCmp);
        }

        /* score test planes  */
        lastquantValue = -1;
        numUniq = MAXCLOSESTCHECK;
        for (nI = lowerQuartile;
             nI <= upperQuartile && numUniq > 0; nI++)
        {
            RwReal              tryValue;
            RwInt32             quantValue;

            /* fetch the next unique distance */
            tryValue = GETCOORD(*(data->sortVerts[nI]->pos), tryPlane);
            quantValue = (RwInt32) (tryValue * quantThresh);
            if (quantValue != lastquantValue)
            {
                clipStatistics      stats;

                /* Set the clip codes */
                SetClipCodes(data->numSortVerts, data->sortVerts,
                             tryPlane, tryValue);

                /* Check what would be resultant polygons with this clip plane              
                 * Including a look at overlaps */
                GetClipStatistics(data, tryPlane, tryValue, &stats);

                /* does it actually divide the geometry? */
                if (stats.nLeft && stats.nRight)
                {
                    RwReal              score;

                    score = DivisionScore(data, &stats, tryPlane);
                    score *= extentScore;

                    /* is it a winner? */
                    if (score < bestScore)
                    {
                        bestScore = score;
                        bestPlane = tryPlane;
                        bestValue = tryValue;
                        bestStats = stats;
                    }
                }

                /* onto next test plane */
                lastquantValue = quantValue;
                numUniq--;
            }
        }
    }

    /* Check if a suitable plane was found at all */
    if (bestScore < RwRealMAXVAL)
    {
        (*plane) = bestPlane;
        (*value) = bestValue;
        RWRETURN(TRUE);
    }

    RWRETURN(FALSE);
}

/******************************************************************************
 *
 *  Sort the polygons according to whether they are in left or right
 *  sector using flags set by GetClipStatistics(). On exit, the
 *  flags are no longer valid.
 */
static void
SortPolygons(buildData * data)
{
    buildPolygon       *polygons = data->polygons + data->polygonOffset;
    RwUInt32            leftCount;
    RwUInt32            iPoly;

    RWFUNCTION(RWSTRING("SortPolygons"));

    leftCount = 0;

    /* For each polygon */
    for (iPoly = 0; iPoly < data->numPolygons; iPoly++)
    {
        /* Is polygon on the left? */
        if (polygons[iPoly].flags == CLIPPOLYGONLEFT)
        {
            /* Move it to first section of array. */
            if (iPoly > leftCount)
            {
                RpCollBSPTriangle  *const swap =
                    polygons[leftCount].poly;

                polygons[leftCount].poly = polygons[iPoly].poly;
                polygons[iPoly].poly = swap;
            }

            leftCount++;
        }
    }

    RWRETURNVOID();
}

/******************************************************************************
 *
 *  BSP leaf node: Polygon sector.
 */
static collBSPPolygonSector *
PolygonSectorCreate(RwUInt32 numPolygons, RwUInt32 firstPolygon)
{
    collBSPPolygonSector *sector;

    RWFUNCTION(RWSTRING("PolygonSectorCreate"));

    sector = (collBSPPolygonSector *)
        RwMalloc(sizeof(collBSPPolygonSector));

    if (!sector)
    {
        RWRETURN(NULL);
    }

    sector->type = -1;
    sector->numPolygons = (RwUInt16) numPolygons;
    sector->iFirstPolygon = (RwUInt16) firstPolygon;

    RWRETURN(sector);
}

/******************************************************************************
 *
 *  BSP tree node: Plane sector.
 */
static collBSPPlaneSector *
PlaneSectorCreate(RwInt32 type, RwReal leftValue, RwReal rightValue)
{
    collBSPPlaneSector *sector;

    RWFUNCTION(RWSTRING("PlaneSectorCreate"));

    sector = (collBSPPlaneSector *)
        RwMalloc(sizeof(collBSPPlaneSector));

    if (!sector)
    {
        RWRETURN(NULL);
    }

    sector->type = type;
    sector->leftValue = leftValue;
    sector->rightValue = rightValue;

    RWRETURN(sector);
}

/******************************************************************************
 * 
 *  Destroy the build tree. May be part built.
 */
static void
BuildTreeDestroy(collBSPSector * sector)
{
    RWFUNCTION(RWSTRING("BuildTreeDestroy"));

    if (sector)
    {
        if (sector->type < 0)
        {
            /* Leaf node */
            RwFree(sector);
        }
        else
        {
            /* Branch node */
            collBSPPlaneSector *planeSector =
                (collBSPPlaneSector *) sector;

            BuildTreeDestroy(planeSector->leftSubTree);
            BuildTreeDestroy(planeSector->rightSubTree);

            RwFree(planeSector);
        }
    }

    RWRETURNVOID();
}

/******************************************************************************
 *
 *  Split the sector recursively, returning a BSP tree.
 */
static collBSPSector *
BuildTreeGenerate(buildData * data)
{
    RwUInt32            plane;
    RwReal              value;

    RWFUNCTION(RWSTRING("BuildTreeGenerate"));
    RWASSERT(data);

    /* First set up pointers to polygon vertices */
    SetSortVertices(data);

    /* Look for dividing pair of planes */
    if (data->bspDepth >= rpCOLLBSP_MAX_DEPTH
        || data->numPolygons < rpCOLLBSP_MIN_POLYGONS_FOR_SPLIT
        || !FindDividingPlane(data, (RwInt32 *) & plane, &value))
    {
        collBSPPolygonSector *polySector;

        /* Make polygon sector */
        polySector = PolygonSectorCreate(data->numPolygons,
                                         data->polygonOffset);
        if (!polySector)
        {
            RWRETURN(NULL);
        }

        RWRETURN((collBSPSector *) polySector);
    }
    else
    {
        buildData           subdata;
        collBSPPlaneSector *planeSector;
        clipStatistics      stats;

        /* Split space */
        data->bspDepth++;

        /* Set the clip codes on the vertices */
        SetClipCodes(data->numSortVerts, data->sortVerts, plane, value);

        /* Check what would be resultant polygons with this clip plane.
         * This flags data->polygons ready for sorting */
        GetClipStatistics(data, plane, value, &stats);

        /* Sort polygons into bunch on left and bunch on right */
        SortPolygons(data);

        /* Create the plane sector */
        planeSector =
            PlaneSectorCreate(plane, stats.leftValue, stats.rightValue);
        if (!planeSector)
        {
            RWRETURN(NULL);
        }

        /* Left polygons */
        subdata = *data;
        subdata.numPolygons = stats.nLeft;
        SETCOORD(subdata.bbox.sup, plane, stats.leftValue);
        planeSector->leftSubTree = BuildTreeGenerate(&subdata);
        if (!planeSector->leftSubTree)
        {
            RwFree(planeSector);
            RWRETURN(NULL);
        }

        /* Right polygons */
        subdata = *data;
        subdata.numPolygons = stats.nRight;
        subdata.polygonOffset = data->polygonOffset + stats.nLeft;
        SETCOORD(subdata.bbox.inf, plane, stats.rightValue);
        planeSector->rightSubTree = BuildTreeGenerate(&subdata);
        if (!planeSector->rightSubTree)
        {
            BuildTreeDestroy((collBSPSector *) planeSector);
            RWRETURN(NULL);
        }

        RWRETURN((collBSPSector *) planeSector);
    }
}

/******************************************************************************
 *
 *  Destroy the build data structure.
 */
static void
BuildDataDestroy(buildData * data)
{
    RWFUNCTION(RWSTRING("BuildDataDestroy"));

    if (data)
    {
        if (data->vertices)
        {
            RwFree(data->vertices);
        }
        if (data->polygons)
        {
            RwFree(data->polygons);
        }
        if (data->sortVerts)
        {
            RwFree(data->sortVerts);
        }
        RwFree(data);
    }

    RWRETURNVOID();
}

/******************************************************************************
 *
 *  Create and initialize the temporary data structure used during
 *  the BSP build process.
 */
static buildData   *
BuildDataCreate(RwInt32 numVertices,
                RwV3d * vertices,
                RwInt32 numPolygons, RpCollBSPTriangle * polygons)
{
    RwInt32             iVert, iPoly;
    buildData          *data;

    RWFUNCTION(RWSTRING("BuildDataCreate"));

    data = (buildData *) RwMalloc(sizeof(buildData));
    if (!data)
    {
        BuildDataDestroy(data);
        RWRETURN(NULL);
    }

    data->vertices =
        (buildVertex *) RwMalloc(numVertices * sizeof(buildVertex));
    if (!data->vertices)
    {
        BuildDataDestroy(data);
        RWRETURN(NULL);
    }

    data->sortVerts =
        (buildVertex **) RwMalloc(numVertices * sizeof(buildVertex *));
    if (!data->sortVerts)
    {
        BuildDataDestroy(data);
        RWRETURN(NULL);
    }

    data->polygons =
        (buildPolygon *) RwMalloc(numPolygons * sizeof(buildPolygon));
    if (!data->polygons)
    {
        BuildDataDestroy(data);
        RWRETURN(NULL);
    }

    /* Initialize data */
    data->numPolygons = numPolygons;
    data->polygonOffset = 0;
    data->numVertices = numVertices;
    data->bspDepth = 0;

    /* Set up bounding box and
     * Initialize pointer tables for vertices and polygons
     */
    RwBBoxInitialize(&data->bbox, &vertices[0]);
    for (iVert = 0; iVert < numVertices; iVert++)
    {
        RwBBoxAddPoint(&data->bbox, &vertices[iVert]);
        data->vertices[iVert].pos = &vertices[iVert];
    }

    for (iPoly = 0; iPoly < numPolygons; iPoly++)
    {
        data->polygons[iPoly].poly = &polygons[iPoly];
    }

    RWRETURN(data);
}

/******************************************************************************
 *  Count the number of leaf nodes in the BSP tree
 */
static              RwInt32
BuildTreeCountLeafNodes(collBSPSector * tree)
{
    collBSPSector      *sector = tree;
    collBSPSector      *sectorStack[rpCOLLBSP_MAX_DEPTH + 1];
    RwInt32             nStack = 0;
    RwInt32             nLeaves = 0;

    RWFUNCTION(RWSTRING("BuildTreeCountLeafNodes"));

    sectorStack[0] = NULL;

    while (sector)
    {
        if (sector->type < 0)  /* Leaf node */
        {
            nLeaves++;
            sector = sectorStack[nStack];
            nStack--;
        }
        else                   /* Branch node */
        {
            collBSPPlaneSector *planeSector =
                (collBSPPlaneSector *) sector;

            /* Put right on stack, and go left */
            nStack++;
            sectorStack[nStack] = planeSector->rightSubTree;
            sector = planeSector->leftSubTree;
        }
    }

    RWRETURN(nLeaves);
}

/******************************************************************************
 *  Flatten tree into self-indexing array. The tree will then
 *  reside in a pointer-free continuous block of memory
 */
static RpCollBSPTree *
BuildTreeConvertNodes(RpCollBSPTree * flatTree,
                      collBSPSector * buildTree,
                      buildData * __RWUNUSEDRELEASE__ data)
{
    RwInt32             nStack = 0;
    RwUInt16           *refStack[rpCOLLBSP_MAX_DEPTH + 1];
    RwUInt16           *reference;
    RwUInt32            iBranch = 0;
    RwUInt32            iLeaf = 0;
    RwUInt32            numTotalPolygons = 0;
    collBSPSector      *sector;
    collBSPSector      *sectorStack[rpCOLLBSP_MAX_DEPTH + 1];
    RwUInt16            dummy;

    RWFUNCTION(RWSTRING("BuildTreeConvertNodes"));

    nStack = 0;
    sector = buildTree;
    reference = &dummy;
    while (nStack >= 0)
    {
        if (sector->type < 0)  /* Leaf node */
        {
            collBSPPolygonSector *polySector =
                (collBSPPolygonSector *) sector;
            RpCollBSPLeafNode  *node = flatTree->leafNodes + iLeaf;

            node->numPolygons = polySector->numPolygons;
            node->firstPolygon = polySector->iFirstPolygon;

            numTotalPolygons += node->numPolygons;

            /* Add reference to this leaf in parent node */
            *reference = (RwUInt16) iLeaf;

            /* Roll back stack */
            sector = sectorStack[nStack];
            reference = refStack[nStack];
            nStack--;

            iLeaf++;
        }
        else                   /* Branch node */
        {
            collBSPPlaneSector *planeSector =
                (collBSPPlaneSector *) sector;
            RpCollBSPBranchNode *node = flatTree->branchNodes + iBranch;

            node->type = (RwUInt16) planeSector->type;
            node->leftValue = planeSector->leftValue;
            node->rightValue = planeSector->rightValue;
            node->leftType = (planeSector->leftSubTree->type < 0)
                ? rpCOLLBSP_LEAF_NODE : rpCOLLBSP_BRANCH_NODE;
            node->rightType = (planeSector->rightSubTree->type < 0)
                ? rpCOLLBSP_LEAF_NODE : rpCOLLBSP_BRANCH_NODE;

            /* Parent must reference this node */
            *reference = (RwUInt16) iBranch;

            /* Put right on stack, and go left */
            nStack++;
            sectorStack[nStack] = planeSector->rightSubTree;
            refStack[nStack] = &node->rightNode;
            sector = planeSector->leftSubTree;
            reference = &node->leftNode;

            iBranch++;
        }
    }

    RWASSERT(iLeaf == flatTree->numLeafNodes);
    RWASSERT(iBranch == flatTree->numLeafNodes - 1);
    RWASSERT(numTotalPolygons == data->numPolygons);
    RWRETURN(flatTree);
}

/******************************************************************************
 *
 *  Generate collision BSP tree from list of vertices and triangles
 */
RpCollBSPTree      *
_rpCollBSPTreeBuild(RwInt32 numVertices,
                    RwV3d * vertices,
                    RwInt32 numTriangles,
                    RpCollBSPTriangle * triangles,
                    RwUInt16 * triangleSortMap)
{
    RpCollBSPTree      *collBSPTree = NULL;
    buildData          *data;

    RWFUNCTION(RWSTRING("_rpCollBSPTreeBuild"));
    RWASSERT(vertices);
    RWASSERT(triangles);
    RWASSERT(triangleSortMap);
    RWASSERT(numTriangles);
    RWASSERT(numVertices);

    /* Create build data structure */
    data =
        BuildDataCreate(numVertices, vertices, numTriangles, triangles);
    if (data)
    {
        collBSPSector      *tree;

        /* Make the build tree (split geometry recursively) */
        tree = BuildTreeGenerate(data);
        if (tree)
        {
            /* Create working data structure */
            collBSPTree =
                _rpCollBSPTreeCreate(BuildTreeCountLeafNodes(tree));
            if (collBSPTree)
            {
                RwInt32             iTri;

                /* Convert build tree to working format */
                BuildTreeConvertNodes(collBSPTree, tree, data);

                /* Build triangle sort map */
                for (iTri = 0; iTri < numTriangles; iTri++)
                {
                    triangleSortMap[iTri] =
                        data->polygons[iTri].poly - triangles;
                }
            }
            BuildTreeDestroy(tree);
        }
        BuildDataDestroy(data);
    }

    RWRETURN(collBSPTree);
}
