/*
 * Collision plugin for Renderware.
 */

/*
 *  File : ctgeom.c
 */

/*****************************************************************************
 *  Include files
 */
#include <rwcore.h>
#include <rpworld.h>
#include <rtintsec.h>

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

#include "ctbsp.h"
#include "ctbuild.h"
#include "ctdef.h"
#include "ctgeom.h"

#if (!defined(DOXYGEN))
static const char rcsid[] __RWUNUSED__ =
    "@@@@(#)$Id: ctgeom.c,v 1.12 2001/05/23 16:22:00 Colinh Exp $";
#endif /* (!defined(DOXYGEN)) */


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

typedef struct GeomCallBack GeomCallBack;
struct GeomCallBack
{
    RpIntersection     *intersection;
    RpIntersectionCallBackGeometryTriangle func;
    void               *data;
};

typedef struct LineCollParam LineCollParam;
struct LineCollParam
{
    RpGeometry         *geometry;
    GeomCallBack       *callBack;
    RwLine             *line;
    RwV3d               delta;
};

typedef struct SphereCollParam SphereCollParam;
struct SphereCollParam
{
    RpGeometry         *geometry;
    GeomCallBack       *callBack;
    RwSphere           *sphere;
    RwReal              recipRadius;
};

typedef struct BBoxCollParam BBoxCollParam;
struct BBoxCollParam
{
    RpGeometry         *geometry;
    GeomCallBack       *callBack;
    RwBBox             *bbox;
};

/*****************************************************************************
 *  Global Variables
 */

/*
 * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 *
 *          Rigid Geometry Intersections (single morph target)
 *
 * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 */

/******************************************************************************
 *
 *  Line test with triangles in BSP leaf node
 */
static              RwBool
GeomLeafNodeForAllLineIntersections(RwInt32 numTriangles,
                                    RwInt32 triOffset, void *data)
{
    LineCollParam      *isData = (LineCollParam *) data;
    RpGeometry         *geometry = isData->geometry;
    RwV3d              *vertices = geometry->morphTarget->verts;
    RpTriangle         *triangles = geometry->triangles;
    GeomCallBack       *cb = isData->callBack;
    RwUInt16           *triIndex =
        (*RPGEOMETRYCOLLISIONDATA(geometry))->triangleMap + triOffset;

    RWFUNCTION(RWSTRING("GeomLeafNodeForAllLineIntersections"));
    RWASSERT(numTriangles >= 0);
    RWASSERT(triOffset >= 0);
    RWASSERT(isData);

    while (numTriangles--)
    {
        RpTriangle         *tri;
        RwV3d              *v0, *v1, *v2;
        RwReal              distance;
        RwBool              result;

        /* Get triangle via map */
        tri = triangles + *triIndex;
        v0 = vertices + tri->vertIndex[0];
        v1 = vertices + tri->vertIndex[1];
        v2 = vertices + tri->vertIndex[2];

        /* Test for collision */
        RtIntersectionLineTriangleMacro(result, &isData->line->start,
                                        &isData->delta, v0, v1, v2,
                                        &distance);

        if (result)
        {
            RpCollisionTriangle collisionTri;

            /* We've got one */
            collisionTri.point = (*v0);
            collisionTri.index = *triIndex;
            _rpTriangleNormalMacro(&collisionTri.normal, v0, v1, v2);
            collisionTri.vertices[0] = v0;
            collisionTri.vertices[1] = v1;
            collisionTri.vertices[2] = v2;

            /* Return normalized distance (0 - 1) */
            if (!cb->
                func(cb->intersection, &collisionTri, distance,
                     cb->data))
            {
                /* Early out */
                RWRETURN(FALSE);
            }
        }

        triIndex++;
    }

    RWRETURN(TRUE);
}

/******************************************************************************
 *
 *  Sphere test with triangles in BSP leaf.
 */
static              RwBool
GeomLeafNodeForAllSphereIntersections(RwInt32 numTriangles,
                                      RwInt32 triOffset, void *data)
{
    SphereCollParam    *isData = (SphereCollParam *) data;
    RpGeometry         *geometry = isData->geometry;
    RwV3d              *vertices = geometry->morphTarget->verts;
    RpTriangle         *triangles = geometry->triangles;
    GeomCallBack       *cb = isData->callBack;
    RwUInt16           *triIndex =
        (*RPGEOMETRYCOLLISIONDATA(geometry))->triangleMap + triOffset;

    RWFUNCTION(RWSTRING("GeomLeafNodeForAllSphereIntersections"));
    RWASSERT(numTriangles >= 0);
    RWASSERT(triOffset >= 0);
    RWASSERT(isData);

    while (numTriangles--)
    {
        RpTriangle         *tri;
        RwV3d              *v0, *v1, *v2;
        RwReal              distance;
        RpCollisionTriangle collisionTri;

        /* Get triangle via map */
        tri = triangles + *triIndex;
        v0 = vertices + tri->vertIndex[0];
        v1 = vertices + tri->vertIndex[1];
        v2 = vertices + tri->vertIndex[2];

        /* Test for collision */
        if (RtIntersectionSphereTriangle(isData->sphere,
                                         v0, v1, v2,
                                         &collisionTri.normal,
                                         &distance))
        {
            /* We've got one */
            collisionTri.point = (*v0);
            collisionTri.index = *triIndex;
            collisionTri.vertices[0] = v0;
            collisionTri.vertices[1] = v1;
            collisionTri.vertices[2] = v2;

            /* Normalize distance with precalculated reciprocal radius */
            distance *= (isData->recipRadius);

            if (!cb->
                func(cb->intersection, &collisionTri, distance,
                     cb->data))
            {
                /* Early out */
                RWRETURN(FALSE);
            }
        }

        triIndex++;
    }

    RWRETURN(TRUE);
}

/******************************************************************************
 *
 *  BBox test with triangles in BSP leaf.
 */
static              RwBool
GeomLeafNodeForAllBBoxIntersections(RwInt32 numTriangles,
                                      RwInt32 triOffset, void *data)
{
    BBoxCollParam      *isData = (BBoxCollParam *) data;
    RpGeometry         *geometry = isData->geometry;
    RwV3d              *vertices = geometry->morphTarget->verts;
    RpTriangle         *triangles = geometry->triangles;
    GeomCallBack       *cb = isData->callBack;
    RwUInt16           *triIndex =
        (*RPGEOMETRYCOLLISIONDATA(geometry))->triangleMap + triOffset;

    RWFUNCTION(RWSTRING("GeomLeafNodeForAllBBoxIntersections"));
    RWASSERT(numTriangles >= 0);
    RWASSERT(triOffset >= 0);
    RWASSERT(isData);

    while (numTriangles--)
    {
        RpTriangle         *tri;
        RwV3d              *v0, *v1, *v2;
        RpCollisionTriangle collisionTri;

        /* Get triangle via map */
        tri = triangles + *triIndex;
        v0 = vertices + tri->vertIndex[0];
        v1 = vertices + tri->vertIndex[1];
        v2 = vertices + tri->vertIndex[2];

        /* Test for collision */
        if (RtIntersectionBBoxTriangle(isData->bbox, v0, v1, v2))
        {
            /* We've got one */
            collisionTri.point = (*v0);
            collisionTri.index = *triIndex;
            _rpTriangleNormalMacro(&collisionTri.normal, v0, v1, v2);
            collisionTri.vertices[0] = v0;
            collisionTri.vertices[1] = v1;
            collisionTri.vertices[2] = v2;

            /* Distance undefined */
            if (!cb->func(cb->intersection, &collisionTri, 0.0f, cb->data))
            {
                /* Early out */
                RWRETURN(FALSE);
            }
        }

        triIndex++;
    }

    RWRETURN(TRUE);
}

/******************************************************************************
 *
 *  Test for intersections between a line and the triangles of a rigid
 *  geometry.
 *
 *  Use BSP data if available, otherwise test all triangles.
 */
static RpGeometry  *
GeometryForAllLineIntersections(RpGeometry * geometry,
                                RwLine * testLine, GeomCallBack * cb)
{
    RpCollisionData    *collData = *RPGEOMETRYCOLLISIONDATA(geometry);

    RWFUNCTION(RWSTRING("GeometryForAllLineIntersections"));
    RWASSERT(geometry);
    RWASSERT(testLine);
    RWASSERT(cb);
    RWASSERT(geometry->numMorphTargets == 1);

    if (collData)
    {
        /* We have BSP data */
        RpV3dGradient       grad;
        LineCollParam isData;

        /* Fill in callback info */
        isData.callBack = cb;
        isData.geometry = geometry;
        isData.line = testLine;

        /* Get the line gradient for BSP clipping */
        RwV3dSub(&isData.delta, &testLine->end, &testLine->start);
        _rpV3dGradientMacro(&grad, &isData.delta);

        /* Now look for intersections */
        _rpCollBSPTreeForAllLineLeafNodeIntersections(collData->tree,
                                                      testLine, &grad,
                                                      GeomLeafNodeForAllLineIntersections,
                                                      &isData);
    }
    else
    {
        /* Test all the triangles */
        RwV3d              *vertices = geometry->morphTarget->verts;
        RpTriangle         *triangle = geometry->triangles;
        RwV3d               delta;
        RwInt32             iTri;

        /* Line delta */
        RwV3dSub(&delta, &testLine->end, &testLine->start);

        for (iTri = 0; iTri < geometry->numTriangles;
             iTri++, triangle++)
        {
            RwV3d              *v0, *v1, *v2;
            RwReal              distance;
            RwBool              result;

            v0 = vertices + triangle->vertIndex[0];
            v1 = vertices + triangle->vertIndex[1];
            v2 = vertices + triangle->vertIndex[2];

            /* Test for collision */
            RtIntersectionLineTriangleMacro(result, &testLine->start,
                                            &delta, v0, v1, v2,
                                            &distance);

            if (result)
            {
                RpCollisionTriangle collisionTri;

                /* We've got one */
                collisionTri.point = (*v0);
                collisionTri.index = iTri;
                _rpTriangleNormalMacro(&collisionTri.normal, v0, v1,
                                       v2);
                collisionTri.vertices[0] = v0;
                collisionTri.vertices[1] = v1;
                collisionTri.vertices[2] = v2;

                if (!cb->
                    func(cb->intersection, &collisionTri, distance,
                         cb->data))
                {
                    /* Early out */
                    RWRETURN(NULL);
                }
            }
        }
    }

    RWRETURN(geometry);
}

/******************************************************************************
 *
 *  Test for intersections between a sphere and the triangles of a rigid
 *  geometry.
 *
 *  Use BSP data if available, otherwise test all triangles.
 */
static RpGeometry  *
GeometryForAllSphereIntersections(RpGeometry * geometry,
                                  RwSphere * testSphere,
                                  GeomCallBack * cb)
{
    RpCollisionData    *collData = *RPGEOMETRYCOLLISIONDATA(geometry);

    RWFUNCTION(RWSTRING("GeometryForAllSphereIntersections"));
    RWASSERT(geometry);
    RWASSERT(testSphere);
    RWASSERT(cb);
    RWASSERT(geometry->numMorphTargets == 1);

    if (collData)
    {
        /* We have BSP data */
        RwBBox              bbox;
        SphereCollParam isData;

        /* Fill in callback info */
        isData.callBack = cb;
        isData.geometry = geometry;
        isData.sphere = testSphere;
        isData.recipRadius = 1.0f / testSphere->radius;

        /* Bounding box */
        _rpSphereBBoxMacro(&bbox, testSphere);

        /* Now look for intersections */
        _rpCollBSPTreeForAllBoxLeafNodeIntersections(collData->tree, &bbox,
            GeomLeafNodeForAllSphereIntersections, &isData);
    }
    else
    {
        /* No BSP, test all the triangles */
        RwV3d              *vertices = geometry->morphTarget->verts;
        RpTriangle         *triangle = geometry->triangles;
        RwReal              recipRadius;
        RwInt32             iTri;

        /* Cache reciprocal radius */
        recipRadius = 1.0f / testSphere->radius;

        for (iTri = 0; iTri < geometry->numTriangles;
             iTri++, triangle++)
        {
            RwV3d              *v0, *v1, *v2;
            RwReal              distance;
            RpCollisionTriangle collisionTri;

            v0 = vertices + triangle->vertIndex[0];
            v1 = vertices + triangle->vertIndex[1];
            v2 = vertices + triangle->vertIndex[2];

            /* Test for collision */
            if (RtIntersectionSphereTriangle(testSphere,
                                             v0, v1, v2,
                                             &collisionTri.normal,
                                             &distance))
            {
                /* We've got one */
                collisionTri.point = (*v0);
                collisionTri.index = iTri;
                collisionTri.vertices[0] = v0;
                collisionTri.vertices[1] = v1;
                collisionTri.vertices[2] = v2;

                /* Normalize distance */
                distance *= recipRadius;

                if (!cb->
                    func(cb->intersection, &collisionTri, distance,
                         cb->data))
                {
                    /* Early out */
                    RWRETURN(NULL);
                }
            }
        }
    }

    RWRETURN(geometry);
}

/******************************************************************************
 *
 *  Test for intersections between a bounding box and the triangles of a rigid
 *  geometry.
 *
 *  Use BSP data if available, otherwise test all triangles.
 */
static RpGeometry  *
GeometryForAllBBoxIntersections(RpGeometry * geometry,
                                  RwBBox * testBBox,
                                  GeomCallBack * cb)
{
    RpCollisionData    *collData = *RPGEOMETRYCOLLISIONDATA(geometry);

    RWFUNCTION(RWSTRING("GeometryForAllBBoxIntersections"));
    RWASSERT(geometry);
    RWASSERT(testBBox);
    RWASSERT(cb);
    RWASSERT(geometry->numMorphTargets == 1);

    if (collData)
    {
        /* We have BSP data */
        BBoxCollParam           isData;

        /* Fill in callback info */
        isData.callBack = cb;
        isData.geometry = geometry;
        isData.bbox = testBBox;

        /* Now look for intersections */
        _rpCollBSPTreeForAllBoxLeafNodeIntersections(collData->tree,
            testBBox, GeomLeafNodeForAllBBoxIntersections, &isData);
    }
    else
    {
        /* No BSP, test all the triangles */
        RwV3d              *vertices = geometry->morphTarget->verts;
        RpTriangle         *triangle = geometry->triangles;
        RwInt32             iTri;

        for (iTri = 0; iTri < geometry->numTriangles;
             iTri++, triangle++)
        {
            RwV3d              *v0, *v1, *v2;
            RpCollisionTriangle collisionTri;

            v0 = vertices + triangle->vertIndex[0];
            v1 = vertices + triangle->vertIndex[1];
            v2 = vertices + triangle->vertIndex[2];

            /* Test for collision */
            if (RtIntersectionBBoxTriangle(testBBox, v0, v1, v2))
            {
                /* We've got one */
                collisionTri.point = (*v0);
                collisionTri.index = iTri;
                _rpTriangleNormalMacro(&collisionTri.normal, v0, v1, v2);
                collisionTri.vertices[0] = v0;
                collisionTri.vertices[1] = v1;
                collisionTri.vertices[2] = v2;

                /* Distance undefined */
                if (!cb->
                    func(cb->intersection, &collisionTri, 0.0f,
                         cb->data))
                {
                    /* Early out */
                    RWRETURN(NULL);
                }
            }
        }
    }

    RWRETURN(geometry);
}

/*
 * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 *
 *          Generic atomic intersections
 *
 * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 */

/******************************************************************************
 *
 *  Line intersections.
 */
static RpAtomic    *
AtomicForAllLineIntersections(RpAtomic * atomic,
                              RwLine * testLine, GeomCallBack * cb)
{
    RpGeometry         *geom = atomic->geometry;
    RwLine              localLine;

    RWFUNCTION(RWSTRING("AtomicForAllLineIntersections"));
    RWASSERT(atomic);
    RWASSERT(testLine);
    RWASSERT(cb);

    /* Transform the line into the frame of the atomic */
    {
        RwMatrix            inverseLTM;

        rwMatrixInitialize(&inverseLTM, rwMATRIXTYPENORMAL);
        RwMatrixInvert(&inverseLTM,
                       RwFrameGetLTM(RpAtomicGetFrame(atomic)));
        RwV3dTransformPoints(&localLine.start, &testLine->start, 2,
                             &inverseLTM);
    }

    if (geom->numMorphTargets == 1)
    {
        /* Pass to rigid geometry test */
        if (!GeometryForAllLineIntersections(geom, &localLine, cb))
        {
            RWRETURN(NULL);
        }
    }
    else
    {
        /* Test all interpolated triangles in the geometry */
        RwV3d               lineDelta;
        RwV3d               v0, v1, v2;
        RwV3d              *vp0 = NULL;
        RwV3d              *vp1 = NULL;
        RwV3d              *vp2 = NULL;
        RpInterpolator     *interpolator = &atomic->interpolator;
        RwV3d              *startFramePoints;
        RwV3d              *endFramePoints;
        RwReal              blendFactor = 0.0f;

        /* Line displacement vector */
        RwV3dSub(&lineDelta, &localLine.end, &localLine.start);

        /* Prepare for key frame interp */
        if ((interpolator->startMorphTarget ==
             interpolator->endMorphTarget)
            || (interpolator->startMorphTarget >= geom->numMorphTargets)
            || (interpolator->endMorphTarget >= geom->numMorphTargets))
        {
            /* This is where we don't have to do any interpolation */
            if ((interpolator->startMorphTarget < geom->numMorphTargets)
                && (interpolator->endMorphTarget <
                    geom->numMorphTargets))
            {
                /* Start and end in range - must be equal, grab morph target */
                startFramePoints =
                    geom->morphTarget[interpolator->startMorphTarget].
                    verts;
            }
            else
            {
                /* Grab key frame zero, one key frame is out of range */
                startFramePoints = geom->morphTarget[0].verts;
            }
            endFramePoints = startFramePoints;
        }
        else
        {
            /* All parameters in range,
             * set up source morph targets and blend factor
             */
            startFramePoints =
                geom->morphTarget[interpolator->startMorphTarget].verts;
            endFramePoints =
                geom->morphTarget[interpolator->endMorphTarget].verts;
            blendFactor =
                (interpolator->recipTime) * (interpolator->position);

            /* We'll need buffers to copy interpolated sectorVertices into */
            vp0 = &v0;
            vp1 = &v1;
            vp2 = &v2;
        }

        /* Interpolate triangles and test */
        {
            RwInt32             iTri;
            RpTriangle         *triangle = geom->triangles;

            for (iTri = 0; iTri < geom->numTriangles; iTri++)
            {
                RwReal              distance;
                RwBool              result;

                if (startFramePoints == endFramePoints)
                {
                    /* Reference sectorVertices from a morph target */
                    vp0 = &startFramePoints[triangle->vertIndex[0]];
                    vp1 = &startFramePoints[triangle->vertIndex[1]];
                    vp2 = &startFramePoints[triangle->vertIndex[2]];
                }
                else
                {
                    /* Interpolate key frames */
                    RwV3d              *startVert, *endVert;

                    startVert =
                        &startFramePoints[triangle->vertIndex[0]];
                    endVert = &endFramePoints[triangle->vertIndex[0]];
                    RwV3dSub(&v0, endVert, startVert);
                    RwV3dScale(&v0, &v0, blendFactor);
                    RwV3dAdd(&v0, &v0, startVert);

                    startVert =
                        &startFramePoints[triangle->vertIndex[1]];
                    endVert = &endFramePoints[triangle->vertIndex[1]];
                    RwV3dSub(&v1, endVert, startVert);
                    RwV3dScale(&v1, &v1, blendFactor);
                    RwV3dAdd(&v1, &v1, startVert);

                    startVert =
                        &startFramePoints[triangle->vertIndex[2]];
                    endVert = &endFramePoints[triangle->vertIndex[2]];
                    RwV3dSub(&v2, endVert, startVert);
                    RwV3dScale(&v2, &v2, blendFactor);
                    RwV3dAdd(&v2, &v2, startVert);
                }

                /* Find out if the line intersects the polygon */
                RtIntersectionLineTriangleMacro(result,
                                                &localLine.start,
                                                &lineDelta, vp0, vp1,
                                                vp2, &distance);

                if (result)
                {
                    /* We have an intersection */
                    RpCollisionTriangle collisionTri;

                    _rpTriangleNormalMacro(&collisionTri.normal, vp0,
                                           vp1, vp2);
                    collisionTri.point = (*vp0);
                    collisionTri.index = iTri;
                    collisionTri.vertices[0] = vp0;
                    collisionTri.vertices[1] = vp1;
                    collisionTri.vertices[2] = vp2;

                    /* Return normalized distance (0 - 1) */
                    if (!cb->func(cb->intersection, &collisionTri,
                                  distance, cb->data))
                    {
                        /* Early out */
                        RWRETURN(atomic);
                    }
                }

                triangle++;
            }
        }
    }

    RWRETURN(atomic);
}

/******************************************************************************
 *
 *  Sphere intersections
 */
static RpAtomic    *
AtomicForAllSphereIntersections(RpAtomic * atomic,
                                const RwSphere * testSphere,
                                GeomCallBack * cb)
{
    RpGeometry         *geom = atomic->geometry;
    RwSphere            localSphere;

    RWFUNCTION(RWSTRING("AtomicForAllSphereIntersections"));
    RWASSERT(atomic);
    RWASSERT(testSphere);
    RWASSERT(cb);

    /* Transform the sphere into the frame of the atomic */
    {
        RwMatrix            inverseLTM;
        RwReal              scaleSq, scale;

        /* Get inverse local transformation matrix */
        rwMatrixInitialize(&inverseLTM, rwMATRIXTYPENORMAL);
        RwMatrixInvert(&inverseLTM,
                       RwFrameGetLTM(RpAtomicGetFrame(atomic)));

        /* Transform the sphere position into the objects space */
        RwV3dTransformPoints(&localSphere.center, &testSphere->center,
                             1, &inverseLTM);

        /* Get sphere radius in object space - assume uniform scaling */
        scaleSq = RwV3dDotProduct(&inverseLTM.at, &inverseLTM.at);
        rwSqrtMacro(scale, scaleSq);
        localSphere.radius = scale * testSphere->radius;
    }

    if (geom->numMorphTargets == 1)
    {
        /* Pass to rigid geometry test */
        if (!GeometryForAllSphereIntersections(geom, &localSphere, cb))
        {
            RWRETURN(NULL);
        }
    }
    else
    {
        /* Test all interpolated triangles in the geometry */
        RwV3d               v0, v1, v2;
        RwV3d              *vp0 = NULL;
        RwV3d              *vp1 = NULL;
        RwV3d              *vp2 = NULL;
        RpInterpolator     *interpolator = &atomic->interpolator;
        RwV3d              *startFramePoints;
        RwV3d              *endFramePoints;
        RwReal              blendFactor = 0.0f;
        RwReal              recipSphereRad;

        /* Cache reciprocal radius */
        recipSphereRad = 1.0f / localSphere.radius;

        /* Prepare for key frame interpolation */
        if ((interpolator->startMorphTarget ==
             interpolator->endMorphTarget)
            || (interpolator->startMorphTarget >= geom->numMorphTargets)
            || (interpolator->endMorphTarget >= geom->numMorphTargets))
        {
            /* This is where we don't have to do any interpolation */
            if ((interpolator->startMorphTarget < geom->numMorphTargets)
                && (interpolator->endMorphTarget <
                    geom->numMorphTargets))
            {
                /* Start and end in range, must be equal - grab morph target */
                startFramePoints =
                    geom->morphTarget[interpolator->startMorphTarget].
                    verts;
            }
            else
            {
                /* Grab key frame zero - one key frame is out of range */
                startFramePoints = geom->morphTarget[0].verts;
            }
            endFramePoints = startFramePoints;
        }
        else
        {
            /* All parameters in range,
             * set up source morph targets and blend factor */
            startFramePoints =
                geom->morphTarget[interpolator->startMorphTarget].verts;
            endFramePoints =
                geom->morphTarget[interpolator->endMorphTarget].verts;
            blendFactor =
                (interpolator->recipTime) * (interpolator->position);

            /* We'll need buffers to copy interpolated sectorVertices into */
            vp0 = &v0;
            vp1 = &v1;
            vp2 = &v2;
        }

        /* Interpolate and test the triangles */
        {
            RwInt32             iTri;
            RpTriangle         *triangle = geom->triangles;

            for (iTri = 0; iTri < geom->numTriangles; iTri++)
            {
                RwReal              distance;
                RpCollisionTriangle collisionTri;

                if (startFramePoints == endFramePoints)
                {
                    /* Reference sectorVertices from a morph target */
                    vp0 = &startFramePoints[triangle->vertIndex[0]];
                    vp1 = &startFramePoints[triangle->vertIndex[1]];
                    vp2 = &startFramePoints[triangle->vertIndex[2]];
                }
                else
                {
                    /* Interpolate key frames */
                    RwV3d              *startVert, *endVert;

                    startVert =
                        &startFramePoints[triangle->vertIndex[0]];
                    endVert = &endFramePoints[triangle->vertIndex[0]];
                    RwV3dSub(&v0, endVert, startVert);
                    RwV3dScale(&v0, &v0, blendFactor);
                    RwV3dAdd(&v0, &v0, startVert);

                    startVert =
                        &startFramePoints[triangle->vertIndex[1]];
                    endVert = &endFramePoints[triangle->vertIndex[1]];
                    RwV3dSub(&v1, endVert, startVert);
                    RwV3dScale(&v1, &v1, blendFactor);
                    RwV3dAdd(&v1, &v1, startVert);

                    startVert =
                        &startFramePoints[triangle->vertIndex[2]];
                    endVert = &endFramePoints[triangle->vertIndex[2]];
                    RwV3dSub(&v2, endVert, startVert);
                    RwV3dScale(&v2, &v2, blendFactor);
                    RwV3dAdd(&v2, &v2, startVert);
                }

                /* Find out if the sphere intersects with the polygon */
                /* This will give us a normal back if it collides */
                if (RtIntersectionSphereTriangle(&localSphere,
                                                 vp0, vp1, vp2,
                                                 &collisionTri.normal,
                                                 &distance))
                {
                    /* Sphere intersects with the polygon,
                     * setup structure and call the callback */
                    collisionTri.point = (*vp0);
                    collisionTri.index = iTri;
                    collisionTri.vertices[0] = vp0;
                    collisionTri.vertices[1] = vp1;
                    collisionTri.vertices[2] = vp2;

                    /* Normalize distance (0 - 1) */
                    distance *= recipSphereRad;

                    if (!cb->func(cb->intersection,
                                  &collisionTri, distance, cb->data))
                    {
                        /* Early out */
                        RWRETURN(atomic);
                    }
                }

                triangle++;
            }
        }
    }

    RWRETURN(atomic);
}

/*
 * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 *
 *          API Interface
 *
 * !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
 */

/**
 * \ingroup rpcollis
 * \ref RpCollisionGeometryBuildData may be called to generate
 * geometry collision extension data for fast intersection tests.
 * This only applies to rigid geometries with a single morph target and
 * the collision data must be rebuilt if the geometry is modified.
 *
 * This function is intended for offline use. The collision data is
 * stream read or written with the geometry.
 *
 * The world and collision plugins must be attached before using this function.
 * Applications should use the header file rpcollis.h and the rpcollis library.
 *
 * \param geometry   Pointer to the geometry
 * \param param   Pointer to the build parameters. This should be set to NULL
 *        as no user parameters are currently supported. A NULL setting will
 *        cause default parameters to be used in future versions.
 *
 * \return Pointer to the geometry if successful, NULL otherwise.
 *
 * \see RpCollisionGeometryForAllIntersections
 * \see RpAtomicForAllIntersections
 * \see RpCollisionWorldBuildData
 * \see RpCollisionPluginAttach
 * \see RpWorldPluginAttach
 */
RpGeometry         *
RpCollisionGeometryBuildData(RpGeometry * geometry,
                             RpCollisionBuildParam * __RWUNUSED__ param)
{
    RpCollisionData   **extData, *collData;

    RWAPIFUNCTION(RWSTRING("RpCollisionGeometryBuildData"));
    RWASSERT(_rpCollisionNumInstances);
    RWASSERT(geometry);
    RWASSERT(geometry->numMorphTargets == 1);
    RWASSERT(geometry->numTriangles);
    RWASSERT(geometry->triangles);
    RWASSERT(geometry->numVertices);
    RWASSERT(geometry->morphTarget);
    RWASSERT(geometry->morphTarget->verts);

    extData = RPGEOMETRYCOLLISIONDATA(geometry);

    /* Destroy any previous collision data */
    if (*extData)
    {
        _rpCollisionDataDestroy(*extData);
    }

    /* Now do generic build of data */
    collData =
        _rpCollisionDataBuild(geometry->numVertices,
                              geometry->morphTarget->verts,
                              geometry->numTriangles,
                              (RpCollBSPTriangle *) geometry->
                              triangles);

    /* Was collision data created successfully? */
    if (!collData)
    {
        RWRETURN(NULL);
    }

    /* Attach to geometry via extension data */
    *extData = collData;

    RWRETURN(geometry);
}

/**
 * \ingroup rpcollis
 * \ref RpCollisionGeometryForAllIntersections is used to test for
 * intersections of the given primitive with all triangles in the
 * specified geometry, which must have only a single morph target.
 * Use \ref RpAtomicForAllIntersections for morphed atomics.
 *
 * Fast elimination of non-intersected triangles is possible
 * if collision extension data is present. Otherwise, all triangles of the
 * geometry are individually tested. See \ref RpCollisionGeometryBuildData.
 *
 * Note that a bounding sphere test is not done before testing the
 * triangles of the geometry, as this usually takes place as part of a first
 * pass collision stage, eg as in \ref RpWorldForAllAtomicIntersections.
 *
 * For each intersection found the given callback function is executed, which
 * may return NULL to terminate intersection testing. See \ref
 * RpIntersectionCallBackGeometryTriangle for details of the information
 * passed to the callback.
 *
 * The world and collision plugin must be attached before using this function.
 * Applications should use the rpcollis.h header and link with the rpcollis
 * and rtintsec libraries.
 *
 * \param geometry  Pointer to the geometry.
 * \param intersection  Pointer to an \ref RpIntersection describing
 *       the intersection primitive, which must be specified in the
 *       coordinate system of the geometry (this may require transformation
 *       from world coordinates using the inverse LTM of the parent atomic's
 *       \ref RwFrame).
 *
 *       Supported intersection types are:
 *
 * \li rpINTERSECTLINE      Line intersections.
 * \li rpINTERSECTSPHERE    Sphere intersections.
 * \li rpINTERSECTBOX       Box intersections.
 * \li rpINTERSECTATOMIC    Atomic intersections based on bounding sphere.
 *
 * \param callBack  Pointer to the callback function.
 * \param data  Pointer to user supplied data to pass to the callback function.
 *
 * \return Returns a pointer to the geometry if successful (even if no
 * intersections were found) or NULL if there is an error.
 *
 * \see RpAtomicForAllIntersections
 * \see RpCollisionGeometryBuildData
 * \see RpCollisionWorldForAllIntersections
 * \see RpCollisionPluginAttach
 * \see RpWorldPluginAttach
 */
RpGeometry         *
RpCollisionGeometryForAllIntersections(RpGeometry * geometry,
                                       RpIntersection * intersection,
                                       RpIntersectionCallBackGeometryTriangle
                                       callBack, void *data)
{
    GeomCallBack        cb;

    RWAPIFUNCTION(RWSTRING("RpCollisionGeometryForAllIntersections"));
    RWASSERT(_rpCollisionNumInstances);
    RWASSERT(geometry);
    RWASSERT(intersection);
    RWASSERT(callBack);

    cb.func = callBack;
    cb.data = data;
    cb.intersection = intersection;

    switch (intersection->type)
    {
        case rpINTERSECTLINE:
            {
                /* Intersect specified line with geometry triangles */
                GeometryForAllLineIntersections(geometry,
                                                &intersection->t.line, &cb);

                RWRETURN(geometry);
            }

        case rpINTERSECTSPHERE:
            {
                /* Intersect specified sphere with geometry triangles */
                GeometryForAllSphereIntersections(geometry,
                                                  &intersection->t.sphere, &cb);

                RWRETURN(geometry);
            }

        case rpINTERSECTBOX:
            {
                /* Intersect specified sphere with geometry triangles */
                GeometryForAllBBoxIntersections(geometry,
                                                &intersection->t.box, &cb);

                RWRETURN(geometry);
            }

        case rpINTERSECTATOMIC:
            {
                /* Intersect atomic bounding sphere with geometry triangles */
                RpAtomic           *atomic =
                    (RpAtomic *) intersection->t.object;

                if (atomic)
                {
                    RwSphere            boundingSphere;

                    RWASSERTISTYPE(atomic, rpATOMIC);

                    /* Get bounding sphere */
                    boundingSphere =
                        *RpAtomicGetWorldBoundingSphere(atomic);

                    GeometryForAllSphereIntersections(geometry,
                                                      &boundingSphere,
                                                      &cb);

                    RWRETURN(geometry);
                }
                else
                {
                    /* Null atomic pointer */
                    RWERROR((E_RW_NULLP));
                    RWRETURN(NULL);
                }
            }

        default:
            {
                /* Unknown or unsupported primitive type */
                RWERROR((E_RP_COLLIS_INV_INTERSECTION));
                RWRETURN(NULL);
            }
    }
}

/**
 * \ingroup rpcollis
 * \ref RpAtomicForAllIntersections is used to test for intersections of
 * the given primitive with all triangles in the geometry of the specified
 * atomic. If the atomic's geometry has multiple morph targets, then
 * the position of each triangle is calculated based on the current
 * \ref RpInterpolator value. If the atomic has a single morph target geometry
 * with collision extension data, then this will be used for greater
 * performance (see \ref RpCollisionGeometryBuildData).
 *
 * For each intersection found the given callback function is executed, which
 * may return NULL to terminate intersection testing. See
 * \ref RpIntersectionCallBackGeometryTriangle for full details of the
 * information passed to the callback.
 *
 * Note that a bounding sphere test is not done before testing the
 * triangles of the atomic, as this usually takes place as part of a first
 * pass collision stage, eg as in \ref RpWorldForAllAtomicIntersections.
 *
 * The world and collision plugins must be attached before using this function.
 * Applications should use the header rpcollis.h and link with the
 * rpcollis and rtintsec libraries.
 *
 * \param atomic  Pointer to the atomic.
 * \param intersection  Pointer to an \ref RpIntersection value describing
 *       the intersection primitive. This must be specified in world-space
 *       coordinates. Supported intersection types are:
 *
 * \li rpINTERSECTLINE      Line intersections.
 * \li rpINTERSECTSPHERE    Sphere intersections.
 * \li rpINTERSECTATOMIC    Atomic intersections based on bounding sphere.
 *
 * \param callBack  Pointer to the callback function.
 * \param data  Pointer to user supplied data to pass to the callback function.
 *
 * \return Returns NULL if there is an error, or otherwise a pointer to the
 * atomic, even if no intersections were found.
 *
 * \see RpCollisionGeometryForAllIntersections
 * \see RpCollisionGeometryBuildData
 * \see RpWorldForAllAtomicIntersections
 * \see RpCollisionWorldForAllIntersections
 * \see RpCollisionPluginAttach
 * \see RpWorldPluginAttach
 */
RpAtomic           *
RpAtomicForAllIntersections(RpAtomic * atomic,
                            RpIntersection * intersection,
                            RpIntersectionCallBackGeometryTriangle
                            callBack, void *data)
{
    GeomCallBack        cb;

    RWAPIFUNCTION(RWSTRING("RpAtomicForAllIntersections"));
    RWASSERT(_rpCollisionNumInstances);
    RWASSERT(atomic);
    RWASSERT(intersection);
    RWASSERT(callBack);

    cb.func = callBack;
    cb.data = data;
    cb.intersection = intersection;

    /* This doesn't need the frame hierarchies to be synchronized.
     * The world space bounding sphere of the atomic is not used,
     * and the individual frames are synchronized as necessary with
     * RwFrameGetLtm().
     */
    switch (intersection->type)
    {
        case rpINTERSECTLINE:
            {
                /* Intersect specified line with atomic's polygons */
                AtomicForAllLineIntersections(atomic,
                                              &intersection->t.line,
                                              &cb);

                RWRETURN(atomic);
            }

        case rpINTERSECTSPHERE:
            {
                /* Intersect specified sphere with atomic's polygons */
                AtomicForAllSphereIntersections(atomic,
                                                &intersection->t.sphere,
                                                &cb);

                RWRETURN(atomic);
            }

        case rpINTERSECTATOMIC:
            {
                /* Intersect bounding sphere of the intersection atomic
                 * with atomic's polygons */
                RpAtomic           *intersectionAtomic = (RpAtomic *)
                    intersection->t.object;

                if (intersectionAtomic)
                {
                    const RwSphere     *boundingSphere;

                    RWASSERTISTYPE(intersectionAtomic, rpATOMIC);

                    /* Get bounding sphere */
                    boundingSphere =
                        RpAtomicGetWorldBoundingSphere
                        (intersectionAtomic);
                    RWASSERT(boundingSphere);

                    AtomicForAllSphereIntersections(atomic,
                                                    boundingSphere,
                                                    &cb);

                    RWRETURN(atomic);
                }
                else
                {
                    /* Null atomic pointer */
                    RWERROR((E_RW_NULLP));
                    RWRETURN(NULL);
                }
            }

        default:
            {
                /* Unknown or unsupported primitive type */
                RWERROR((E_RP_COLLIS_INV_INTERSECTION));
                RWRETURN(NULL);
            }
    }
}
