/*
 *
 * Collision plugin for Renderware.
 *
 */

/*
 *  File : ctbsp.c
 *
 *  The collision BSP tree object.
 *  BSP construction is dealt with in ctbuild.c
 */

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

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

#include "ctbsp.h"

#if (!defined(DOXYGEN))
static const char rcsid[] __RWUNUSED__ =
    "@@@@(#)$Id: ctbsp.c,v 1.10 2001/04/02 13:06:21 johns Exp $";
#endif /* (!defined(DOXYGEN)) */


/******************************************************************************
 *  Functions
 */

/*
 * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 *
 *      Creation/destruction of BSP tree datastructure
 *
 * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 */

/******************************************************************************
 *
 *  Initialize a collision BSP tree in a single block of memory.
 */
RpCollBSPTree      *
_rpCollBSPTreeInit(RpCollBSPTree * tree, RwInt32 numLeafNodes)
{
    RwInt32             numBranchNodes;

    RWFUNCTION(RWSTRING("_rpCollBSPTreeInit"));

    /* Note: number of branch nodes is one less than number of leaf
     * nodes in binary tree.
     */
    numBranchNodes = numLeafNodes - 1;

    /* Fill in data and pointers */
    tree->numLeafNodes = numLeafNodes;

    if (numBranchNodes > 0)
    {
        tree->branchNodes = (RpCollBSPBranchNode *) (tree + 1);
        tree->leafNodes = (RpCollBSPLeafNode *)
            (tree->branchNodes + numBranchNodes);
    }
    else
    {
        tree->branchNodes = NULL;
        tree->leafNodes = (RpCollBSPLeafNode *) (tree + 1);
    }

    RWRETURN(tree);
}

/******************************************************************************
 *
 *  Return amount of memory in bytes required to store a collision BSP tree.
 *  This may be used in conjunction with _rpCollBSPTreeInit to store
 *  a tree in externally allocated memory.
 *
 *  The result is a multiple of 4 bytes.
 */
RwInt32
_rpCollBSPTreeMemGetSize(RwInt32 numLeafNodes)
{
    RwInt32             size;

    RWFUNCTION(RWSTRING("_rpCollBSPTreeMemGetSize"));

    /* Number of branch nodes is one less than leaf nodes */
    size = sizeof(RpCollBSPTree)
        + (numLeafNodes - 1) * sizeof(RpCollBSPBranchNode)
        + numLeafNodes * sizeof(RpCollBSPLeafNode);

    RWRETURN(size);
}

/******************************************************************************
 *  
 *  Destroy BSP tree. Only for malloced trees.
 */
void
_rpCollBSPTreeDestroy(RpCollBSPTree * tree)
{
    RWFUNCTION(RWSTRING("_rpCollBSPTreeDestroy"));

    if (tree)
    {
        /* Created with single Malloc */
        RwFree(tree);
    }

    RWRETURNVOID();
}

/******************************************************************************
 *
 *  Create empty collision BSP tree with single malloc.
 */
RpCollBSPTree      *
_rpCollBSPTreeCreate(RwInt32 numLeafNodes)
{
    RpCollBSPTree      *tree;
    RwInt32             size;

    RWFUNCTION(RWSTRING("_rpCollBSPTreeCreate"));
    RWASSERT(numLeafNodes);

    /* Setup single malloc  - everything should be 4 byte aligned */
    size = _rpCollBSPTreeMemGetSize(numLeafNodes);

    tree = (RpCollBSPTree *) RwMalloc(size);
    if (!tree)
    {
        RWRETURN(NULL);
    }

    /* Set up pointers in structure */
    _rpCollBSPTreeInit(tree, numLeafNodes);

    RWRETURN(tree);
}

/*
 * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 *
 *      Stream read/write/size
 *
 * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 */

/****************************************************************************
 *
 *  Serialise collision BSP tree nodes.
 */
const RpCollBSPTree *
_rpCollBSPTreeStreamWrite(const RpCollBSPTree * tree, RwStream * stream)
{
    RwUInt32            numLeaves;
    RwUInt32            numBranches;
    RpCollBSPBranchNode *branch;
    RpCollBSPLeafNode  *leaf;

    RWFUNCTION(RWSTRING("_rpCollBSPTreeStreamWrite"));
    RWASSERT(tree);
    RWASSERT(stream);

    /* Branches */
    branch = tree->branchNodes;
    numBranches = tree->numLeafNodes - 1;
    while (numBranches--)
    {
        RwUInt32            types, indices;

        /* Put branch type and child node types in single word */
        types = branch->type << 16
            | branch->leftType << 8 | branch->rightType;

        /* Put left and right node indices in single word */
        indices = branch->leftNode << 16 | branch->rightNode;

        /* Write branch node data */
        if (!(RwStreamWriteInt(stream,
                               (const RwInt32 *) &types, sizeof(types))
              && RwStreamWriteInt(stream,
                                  (const RwInt32 *) &indices,
                                  sizeof(indices))
              && RwStreamWriteReal(stream,
                                   (const RwReal *) &branch->leftValue,
                                   sizeof(RwReal))
              && RwStreamWriteReal(stream,
                                   (const RwReal *) &branch->rightValue,
                                   sizeof(RwReal))))
        {
            /* Failure */
            RWRETURN(NULL);
        }

        branch++;
    }

    /* Leaves */
    numLeaves = tree->numLeafNodes;
    leaf = tree->leafNodes;
    while (numLeaves--)
    {
        RwUInt32            data;

        /* Put number of polygons and first index in single word */
        data = leaf->numPolygons << 16 | leaf->firstPolygon;

        if (!RwStreamWriteInt
            (stream, (const RwInt32 *) &data, sizeof(data)))
        {
            RWRETURN(NULL);
        }

        leaf++;
    }

    /* All done */
    RWRETURN(tree);
}

/****************************************************************************
 *
 *  Read collision BSP tree from stream. tree must be initialized
 */
RpCollBSPTree      *
_rpCollBSPTreeStreamRead(RpCollBSPTree * tree, RwStream * stream)
{
    RwUInt32            numLeaves;
    RwUInt32            numBranches;
    RpCollBSPBranchNode *branch;
    RpCollBSPLeafNode  *leaf;

    RWFUNCTION(RWSTRING("_rpCollBSPTreeStreamRead"));
    RWASSERT(stream);
    RWASSERT(tree);
    RWASSERT(tree->numLeafNodes > 0);

    /* Branches */
    branch = tree->branchNodes;
    numBranches = tree->numLeafNodes - 1;
    while (numBranches--)
    {
        RwUInt32            types, indices;

        /* Read branch node data */
        if (!
            (RwStreamReadInt(stream, (RwInt32 *) & types, sizeof(types))
             && RwStreamReadInt(stream, (RwInt32 *) & indices,
                                sizeof(indices))
             && RwStreamReadReal(stream, &branch->leftValue,
                                 sizeof(RwReal))
             && RwStreamReadReal(stream, &branch->rightValue,
                                 sizeof(RwReal))))
        {
            /* Failure */
            RWRETURN(NULL);
        }

        /* Get branch type and child node types from single word */
        branch->type = (RwUInt16) ((types >> 16) & 0xffff);
        branch->leftType = (RwUInt8) ((types >> 8) & 0xff);
        branch->rightType = (RwUInt8) (types & 0xff);

        /* Get left and right node indices from single word */
        branch->leftNode = (RwUInt16) ((indices >> 16) & 0xffff);
        branch->rightNode = (RwUInt16) (indices & 0xffff);

        branch++;
    }

    /* Leaves */
    numLeaves = tree->numLeafNodes;
    leaf = tree->leafNodes;
    while (numLeaves--)
    {
        RwUInt32            data;

        /* Read leaf data */
        if (!RwStreamReadInt(stream, (RwInt32 *) & data, sizeof(data)))
        {
            _rpCollBSPTreeDestroy(tree);
            RWRETURN(NULL);
        }

        /* Get number of polygons and first index from single word */
        leaf->numPolygons = (RwUInt16) ((data >> 16) & 0xffff);
        leaf->firstPolygon = (RwUInt16) (data & 0xffff);

        leaf++;
    }

    /* All done */
    RWRETURN(tree);
}

/****************************************************************************
 *
 *  Get size of binary stream data for collision BSP tree.
 */
RwInt32
_rpCollBSPTreeStreamGetSize(const RpCollBSPTree * tree)
{
    RwInt32             size;

    RWFUNCTION(RWSTRING("_rpCollBSPTreeStreamGetSize"));
    RWASSERT(tree);

    /* Branches */
    size =
        (tree->numLeafNodes - 1) * (2 * sizeof(RwInt32) +
                                    2 * sizeof(RwReal));

    /* Leaves */
    size += tree->numLeafNodes * sizeof(RwInt32);

    RWRETURN(size);
}

/******************************************************************************
 *
 *  Translate the partition planes in a BSP tree. This may be used to
 *  update collision data if a world/geometry is translated.
 */
RpCollBSPTree      *
_rpCollBSPTreeTranslate(RpCollBSPTree * tree, const RwV3d * offset)
{
    RwInt32             numBranches;
    RpCollBSPBranchNode *branch;

    RWFUNCTION(RWSTRING("_rpCollBSPTreeTranslate"));

    numBranches = tree->numLeafNodes - 1;
    branch = tree->branchNodes;

    while (numBranches--)
    {
        RwReal              delta;

        /* Get offset value for relevant axis, and translate */
        delta = GETCONSTCOORD(*offset, branch->type);
        branch->leftValue += delta;
        branch->rightValue += delta;

        branch++;
    }

    RWRETURN(tree);
}

/*
 * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 *
 *      Collision tests on BSP tree    
 *
 * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 */

typedef struct nodeInfo nodeInfo;
struct nodeInfo
{
    RwUInt32            type;
    RwUInt32            index;
};

/******************************************************************************
 *
 *  Line intersections with collision BSP leaf nodes
 *
 *  Early out with NULL if callback returns NULL.
 */
RpCollBSPTree      *
_rpCollBSPTreeForAllLineLeafNodeIntersections(RpCollBSPTree * tree,
                                              RwLine * line,
                                              RpV3dGradient * grad,
                                              RpCollBSPLeafCB callBack,
                                              void *data)
{
    /* Need data stack for recursion */
    RwInt32             nStack;
    nodeInfo            nodeStack[rpCOLLBSP_MAX_DEPTH + 1], node;
    RwLine              lineStack[rpCOLLBSP_MAX_DEPTH + 1], currLine;

    RWFUNCTION(RWSTRING("_rpCollBSPTreeForAllLineLeafNodeIntersections"));
    RWASSERT(tree);
    RWASSERT(line);
    RWASSERT(grad);
    RWASSERT(callBack);

    /* Go down tree recursively */
    node.type =
        tree->branchNodes ? rpCOLLBSP_BRANCH_NODE : rpCOLLBSP_LEAF_NODE;
    node.index = 0;
    currLine = *line;
    nStack = 0;

    while (nStack >= 0)
    {
        if (node.type == rpCOLLBSP_LEAF_NODE)
        {
            RpCollBSPLeafNode  *leaf;

            leaf = tree->leafNodes + node.index;
            if (!callBack(leaf->numPolygons, leaf->firstPolygon, data))
            {
                RWRETURN(NULL);
            }

            /* Unstack */
            node = nodeStack[nStack];
            currLine = lineStack[nStack];
            nStack--;
        }
        else
        {
            RwSplitBits         lStart, lEnd;
            RwSplitBits         rStart, rEnd;
            RpCollBSPBranchNode *branch;

/* Local macro to aid readability */
#define PUSH_NODE_INFO(_type, _index, lineStart, lineEnd)       \
    MACRO_START                                                 \
    {                                                           \
        nStack++;                                               \
        nodeStack[nStack].type = (_type);                       \
        nodeStack[nStack].index = (_index);                     \
        lineStack[nStack].start = (lineStart);                  \
        lineStack[nStack].end = (lineEnd);                      \
    }                                                           \
    MACRO_STOP

            /* Its a plane, find out which way we need to go */
            branch = tree->branchNodes + node.index;

            /* Find out where line end points are in relation to the plane
             * Note: leftValue > rightValue as these mean value for left and right
             * sector respectively. 
             */
            lStart.nReal = GETCOORD(currLine.start, branch->type)
                - branch->leftValue;
            lEnd.nReal = GETCOORD(currLine.end, branch->type)
                - branch->leftValue;
            rStart.nReal = GETCOORD(currLine.start, branch->type)
                - branch->rightValue;
            rEnd.nReal = GETCOORD(currLine.end, branch->type)
                - branch->rightValue;

            /* First test if it's entirely one side or the other 
             * Note that the line can never lie in the plane, because we use 
             * the sign bits to determine which sides the ends are on, so zero
             * means on the right.
             */
            if (rStart.nInt < 0 && rEnd.nInt < 0)
            {
                /* totally left */
                node.type = branch->leftType;
                node.index = branch->leftNode;
            }
            else if (lStart.nInt >= 0 && lEnd.nInt >= 0)
            {
                /* totally right */
                node.type = branch->rightType;
                node.index = branch->rightNode;
            }
            else if (!((lStart.nInt ^ lEnd.nInt) & 0x80000000) &&
                     !((rStart.nInt ^ rEnd.nInt) & 0x80000000))
            {
                /* Doesn't cross either, sat in overlap regions */
                if (rStart.nInt < rEnd.nInt)
                {
                    /* go left, stack right */
                    PUSH_NODE_INFO(branch->rightType, branch->rightNode,
                                   currLine.start, currLine.end);
                    node.type = branch->leftType;
                    node.index = branch->leftNode;
                }
                else
                {
                    /* go right, stack left */
                    PUSH_NODE_INFO(branch->leftType, branch->leftNode,
                                   currLine.start, currLine.end);
                    node.type = branch->rightType;
                    node.index = branch->rightNode;
                }
            }
            else
            {
                if (((lStart.nInt ^ lEnd.nInt) & 0x80000000) &&
                    (rStart.nInt >= 0 && rEnd.nInt >= 0))
                {
                    /* crosses left and totally in right */
                    RwV3d               vTmp;

                    /* Calculate an intersection point for left plane */
                    _rpLinePlaneIntersectMacro(&vTmp, &currLine, grad,
                                               branch->type,
                                               branch->leftValue);

                    if (lStart.nInt < 0)
                    {
                        /* stack right go left */
                        PUSH_NODE_INFO(branch->rightType,
                                       branch->rightNode,
                                       currLine.start, currLine.end);
                        node.type = branch->leftType;
                        node.index = branch->leftNode;
                        currLine.end = vTmp;
                    }
                    else
                    {
                        /* stack left go right */
                        PUSH_NODE_INFO(branch->leftType,
                                       branch->leftNode, vTmp,
                                       currLine.end);
                        node.type = branch->rightType;
                        node.index = branch->rightNode;
                    }
                }
                else if (((rStart.nInt ^ rEnd.nInt) & 0x80000000) &&
                         (lStart.nInt < 0 && lEnd.nInt < 0))
                {
                    /* crosses right and totally in left */
                    RwV3d               vTmp;

                    /* Calculate an intersection point for right plane */
                    _rpLinePlaneIntersectMacro(&vTmp, &currLine, grad,
                                               branch->type,
                                               branch->rightValue);

                    if (rStart.nInt < 0)
                    {
                        /* stack right go left */
                        PUSH_NODE_INFO(branch->rightType,
                                       branch->rightNode, vTmp,
                                       currLine.end);
                        node.type = branch->leftType;
                        node.index = branch->leftNode;
                    }
                    else
                    {
                        /* stack left go right */
                        PUSH_NODE_INFO(branch->leftType,
                                       branch->leftNode, currLine.start,
                                       currLine.end);
                        node.type = branch->rightType;
                        node.index = branch->rightNode;
                        currLine.end = vTmp;
                    }
                }
                else
                {
                    /* Must cross both planes */
                    RwV3d               vLeft, vRight;

                    /* Calc intersections at planes */
                    _rpLinePlaneIntersectMacro(&vLeft, &currLine, grad,
                                               branch->type,
                                               branch->leftValue);

                    _rpLinePlaneIntersectMacro(&vRight, &currLine, grad,
                                               branch->type,
                                               branch->rightValue);

                    if (lStart.nInt < 0)
                    {
                        /* Stack right, go left */
                        PUSH_NODE_INFO(branch->rightType,
                                       branch->rightNode, vRight,
                                       currLine.end);
                        node.type = branch->leftType;
                        node.index = branch->leftNode;
                        currLine.end = vLeft;
                    }
                    else
                    {
                        /* Stack left, go right */
                        /* Calc intersection for left plane and stack */
                        PUSH_NODE_INFO(branch->leftType,
                                       branch->leftNode, vLeft,
                                       currLine.end);
                        node.type = branch->rightType;
                        node.index = branch->rightNode;
                        currLine.end = vRight;
                    }
                }
            }
        }
    }

    /* All done */
    RWRETURN(tree);
}

/******************************************************************************
 *
 *  BOX intersections with collision BSP leaf
 * 
 *  Early out with NULL if callBack returns NULL.
 */
RpCollBSPTree      *
_rpCollBSPTreeForAllBoxLeafNodeIntersections(RpCollBSPTree * tree,
                                             RwBBox * box,
                                             RpCollBSPLeafCB callBack,
                                             void *data)
{
    RwInt32             nStack;
    nodeInfo            nodeStack[rpCOLLBSP_MAX_DEPTH + 1], node;

    RWFUNCTION(RWSTRING("_rpCollBSPTreeForAllBoxLeafNodeIntersections"));
    RWASSERT(tree);
    RWASSERT(box);
    RWASSERT(callBack);

    /* Go down tree recursively */
    node.type =
        tree->branchNodes ? rpCOLLBSP_BRANCH_NODE : rpCOLLBSP_LEAF_NODE;
    node.index = 0;
    nStack = 0;
    while (nStack >= 0)
    {
        if (node.type == rpCOLLBSP_LEAF_NODE)
        {
            RpCollBSPLeafNode  *leaf;

            leaf = tree->leafNodes + node.index;
            if (!callBack(leaf->numPolygons, leaf->firstPolygon, data))
            {
                RWRETURN(NULL);
            }

            /* Unstack */
            node = nodeStack[nStack];
            nStack--;
        }
        else
        {
            RpCollBSPBranchNode *branch;

            /* Its a plane, find out which way we need to go */
            branch = tree->branchNodes + node.index;

            if (GETCOORD(box->inf, branch->type) < branch->leftValue)
            {
                /* Go left */
                node.type = branch->leftType;
                node.index = branch->leftNode;

                if (GETCOORD(box->sup, branch->type) >=
                    branch->rightValue)
                {
                    /* Also in right, so stack */
                    nStack++;
                    nodeStack[nStack].index = branch->rightNode;
                    nodeStack[nStack].type = branch->rightType;
                }
            }
            else
            {
                /* Must be completely on right */
                node.type = branch->rightType;
                node.index = branch->rightNode;
            }
        }
    }

    /* All done */
    RWRETURN(tree);
}
