#include "RaycastMesh.h"
#include <assert.h>
#include <math.h>
#include <vector>

#pragma warning(disable : 4100)

namespace RAYCAST_MESH
{
    typedef std::vector<RmUint32> TriVector;

#define RAYAABB_EPSILON 0.00001f

#define IR(x) ((RmUint32&)x)

    bool intersectRayAABB(const RmReal MinB[3], const RmReal MaxB[3], const RmReal origin[3], const RmReal dir[3], RmReal coord[3])
    {
        bool   Inside = true;
        RmReal MaxT[3];
        MaxT[0] = MaxT[1] = MaxT[2] = -1.0f;

        for (RmUint32 i = 0; i < 3; i++)
        {
            if (origin[i] < MinB[i])
            {
                coord[i] = MinB[i];
                Inside   = false;

                if (IR(dir[i]))
                    MaxT[i] = (MinB[i] - origin[i]) / dir[i];
            }
            else if (origin[i] > MaxB[i])
            {
                coord[i] = MaxB[i];
                Inside   = false;

                if (IR(dir[i]))
                    MaxT[i] = (MaxB[i] - origin[i]) / dir[i];
            }
        }

        if (Inside)
        {
            coord[0] = origin[0];
            coord[1] = origin[1];
            coord[2] = origin[2];
            return true;
        }

        RmUint32 WhichPlane = 0;
        if (MaxT[1] > MaxT[WhichPlane])
            WhichPlane = 1;
        if (MaxT[2] > MaxT[WhichPlane])
            WhichPlane = 2;

        if (IR(MaxT[WhichPlane]) & 0x80000000)
            return false;

        for (RmUint32 i = 0; i < 3; i++)
        {
            if (i != WhichPlane)
            {
                coord[i] = origin[i] + MaxT[WhichPlane] * dir[i];
#ifdef RAYAABB_EPSILON
                if (coord[i] < MinB[i] - RAYAABB_EPSILON || coord[i] > MaxB[i] + RAYAABB_EPSILON)
                    return false;
#else
                if (coord[i] < MinB[i] || coord[i] > MaxB[i])
                    return false;
#endif
            }
        }
        return true; 
    }

    bool intersectLineSegmentAABB(const RmReal bmin[3], const RmReal bmax[3], const RmReal p1[3], const RmReal dir[3], RmReal& dist, RmReal intersect[3])
    {
        bool ret = false;

        if (dist > RAYAABB_EPSILON)
        {
            ret = intersectRayAABB(bmin, bmax, p1, dir, intersect);
            if (ret)
            {
                RmReal dx = p1[0] - intersect[0];
                RmReal dy = p1[1] - intersect[1];
                RmReal dz = p1[2] - intersect[2];
                RmReal d  = dx * dx + dy * dy + dz * dz;
                if (d < dist * dist)
                {
                    dist = sqrtf(d);
                }
                else
                {
                    ret = false;
                }
            }
        }
        return ret;
    }

#define vector(a, b, c)       \
    (a)[0] = (b)[0] - (c)[0]; \
    (a)[1] = (b)[1] - (c)[1]; \
    (a)[2] = (b)[2] - (c)[2];

#define innerProduct(v, q) \
    ((v)[0] * (q)[0] +     \
     (v)[1] * (q)[1] +     \
     (v)[2] * (q)[2])

#define crossProduct(a, b, c)                   \
    (a)[0] = (b)[1] * (c)[2] - (c)[1] * (b)[2]; \
    (a)[1] = (b)[2] * (c)[0] - (c)[2] * (b)[0]; \
    (a)[2] = (b)[0] * (c)[1] - (c)[0] * (b)[1];

    static inline bool rayIntersectsTriangle(const RmReal* p, const RmReal* d, const RmReal* v0, const RmReal* v1, const RmReal* v2, RmReal& t)
    {
        RmReal e1[3], e2[3], h[3], s[3], q[3];
        RmReal a, f, u, v;

        vector(e1, v1, v0);
        vector(e2, v2, v0);
        crossProduct(h, d, e2);
        a = innerProduct(e1, h);

        if (a > -0.00001 && a < 0.00001)
            return (false);

        f = 1 / a;
        vector(s, p, v0);
        u = f * (innerProduct(s, h));

        if (u < 0.0 || u > 1.0)
            return (false);

        crossProduct(q, s, e1);
        v = f * innerProduct(d, q);
        if (v < 0.0 || u + v > 1.0)
            return (false);

        t = f * innerProduct(e2, q);
        if (t > 0) 
            return (true);
        else 

            return (false);
    }

    static RmReal computePlane(const RmReal* A, const RmReal* B, const RmReal* C, RmReal* n) 
    {
        RmReal vx = (B[0] - C[0]);
        RmReal vy = (B[1] - C[1]);
        RmReal vz = (B[2] - C[2]);

        RmReal wx = (A[0] - B[0]);
        RmReal wy = (A[1] - B[1]);
        RmReal wz = (A[2] - B[2]);

        RmReal vw_x = vy * wz - vz * wy;
        RmReal vw_y = vz * wx - vx * wz;
        RmReal vw_z = vx * wy - vy * wx;

        RmReal mag = sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));

        if (mag < 0.000001f)
        {
            mag = 0;
        }
        else
        {
            mag = 1.0f / mag;
        }

        RmReal x = vw_x * mag;
        RmReal y = vw_y * mag;
        RmReal z = vw_z * mag;

        RmReal D = 0.0f - ((x * A[0]) + (y * A[1]) + (z * A[2]));

        n[0] = x;
        n[1] = y;
        n[2] = z;

        return D;
    }

#define TRI_EOF 0xFFFFFFFF

    enum AxisAABB
    {
        AABB_XAXIS,
        AABB_YAXIS,
        AABB_ZAXIS
    };

    enum ClipCode
    {
        CC_MINX = (1 << 0),
        CC_MAXX = (1 << 1),
        CC_MINY = (1 << 2),
        CC_MAXY = (1 << 3),
        CC_MINZ = (1 << 4),
        CC_MAXZ = (1 << 5),
    };

    class BoundsAABB
    {
    public:
        void setMin(const RmReal* v)
        {
            mMin[0] = v[0];
            mMin[1] = v[1];
            mMin[2] = v[2];
        }

        void setMax(const RmReal* v)
        {
            mMax[0] = v[0];
            mMax[1] = v[1];
            mMax[2] = v[2];
        }

        void setMin(RmReal x, RmReal y, RmReal z)
        {
            mMin[0] = x;
            mMin[1] = y;
            mMin[2] = z;
        }

        void setMax(RmReal x, RmReal y, RmReal z)
        {
            mMax[0] = x;
            mMax[1] = y;
            mMax[2] = z;
        }

        void include(const RmReal* v)
        {
            if (v[0] < mMin[0])
                mMin[0] = v[0];
            if (v[1] < mMin[1])
                mMin[1] = v[1];
            if (v[2] < mMin[2])
                mMin[2] = v[2];

            if (v[0] > mMax[0])
                mMax[0] = v[0];
            if (v[1] > mMax[1])
                mMax[1] = v[1];
            if (v[2] > mMax[2])
                mMax[2] = v[2];
        }

        void getCenter(RmReal* center) const
        {
            center[0] = (mMin[0] + mMax[0]) * 0.5f;
            center[1] = (mMin[1] + mMax[1]) * 0.5f;
            center[2] = (mMin[2] + mMax[2]) * 0.5f;
        }

        bool intersects(const BoundsAABB& b) const
        {
            if ((mMin[0] > b.mMax[0]) || (b.mMin[0] > mMax[0]))
                return false;
            if ((mMin[1] > b.mMax[1]) || (b.mMin[1] > mMax[1]))
                return false;
            if ((mMin[2] > b.mMax[2]) || (b.mMin[2] > mMax[2]))
                return false;
            return true;
        }

        bool containsTriangle(const RmReal* p1, const RmReal* p2, const RmReal* p3) const
        {
            BoundsAABB b;
            b.setMin(p1);
            b.setMax(p1);
            b.include(p2);
            b.include(p3);
            return intersects(b);
        }

        bool containsTriangleExact(const RmReal* p1, const RmReal* p2, const RmReal* p3, RmUint32& orCode) const
        {
            bool ret = false;

            RmUint32 andCode;
            orCode = getClipCode(p1, p2, p3, andCode);
            if (andCode == 0)
            {
                ret = true;
            }

            return ret;
        }

        inline RmUint32 getClipCode(const RmReal* p1, const RmReal* p2, const RmReal* p3, RmUint32& andCode) const
        {
            andCode     = 0xFFFFFFFF;
            RmUint32 c1 = getClipCode(p1);
            RmUint32 c2 = getClipCode(p2);
            RmUint32 c3 = getClipCode(p3);
            andCode &= c1;
            andCode &= c2;
            andCode &= c3;
            return c1 | c2 | c3;
        }

        inline RmUint32 getClipCode(const RmReal* p) const
        {
            RmUint32 ret = 0;

            if (p[0] < mMin[0])
            {
                ret |= CC_MINX;
            }
            else if (p[0] > mMax[0])
            {
                ret |= CC_MAXX;
            }

            if (p[1] < mMin[1])
            {
                ret |= CC_MINY;
            }
            else if (p[1] > mMax[1])
            {
                ret |= CC_MAXY;
            }

            if (p[2] < mMin[2])
            {
                ret |= CC_MINZ;
            }
            else if (p[2] > mMax[2])
            {
                ret |= CC_MAXZ;
            }

            return ret;
        }

        inline void clamp(const BoundsAABB& aabb)
        {
            if (mMin[0] < aabb.mMin[0])
                mMin[0] = aabb.mMin[0];
            if (mMin[1] < aabb.mMin[1])
                mMin[1] = aabb.mMin[1];
            if (mMin[2] < aabb.mMin[2])
                mMin[2] = aabb.mMin[2];
            if (mMax[0] > aabb.mMax[0])
                mMax[0] = aabb.mMax[0];
            if (mMax[1] > aabb.mMax[1])
                mMax[1] = aabb.mMax[1];
            if (mMax[2] > aabb.mMax[2])
                mMax[2] = aabb.mMax[2];
        }

        RmReal mMin[3];
        RmReal mMax[3];
    };

    class NodeAABB;

    class NodeInterface
    {
    public:
        virtual NodeAABB* getNode(void)                                   = 0;
        virtual void      getFaceNormal(RmUint32 tri, RmReal* faceNormal) = 0;
    };

    class NodeAABB
    {
    public:
        NodeAABB(void)
        {
            mLeft              = NULL;
            mRight             = NULL;
            mLeafTriangleIndex = TRI_EOF;
        }

        NodeAABB(RmUint32 vcount, const RmReal* vertices, RmUint32 tcount, RmUint32* indices,
                 RmUint32       maxDepth,    
                 RmUint32       minLeafSize, 
                 RmReal         minAxisSize,
                 NodeInterface* callback,
                 TriVector&     leafTriangles) 

        {
            mLeft              = NULL;
            mRight             = NULL;
            mLeafTriangleIndex = TRI_EOF;
            TriVector triangles;
            triangles.reserve(tcount);
            for (RmUint32 i = 0; i < tcount; i++)
            {
                triangles.push_back(i);
            }
            mBounds.setMin(vertices);
            mBounds.setMax(vertices);
            const RmReal* vtx = vertices + 3;
            for (RmUint32 i = 1; i < vcount; i++)
            {
                mBounds.include(vtx);
                vtx += 3;
            }
            split(triangles, vcount, vertices, tcount, indices, 0, maxDepth, minLeafSize, minAxisSize, callback, leafTriangles);
        }

        NodeAABB(const BoundsAABB& aabb)
        {
            mBounds            = aabb;
            mLeft              = NULL;
            mRight             = NULL;
            mLeafTriangleIndex = TRI_EOF;
        }

        ~NodeAABB(void)
        {
        }

        void split(const TriVector& triangles,
                   RmUint32         vcount,
                   const RmReal*    vertices,
                   RmUint32         tcount,
                   const RmUint32*  indices,
                   RmUint32         depth,
                   RmUint32         maxDepth,    
                   RmUint32         minLeafSize, 
                   RmReal           minAxisSize,
                   NodeInterface*   callback,
                   TriVector&       leafTriangles) 

        {

            RmReal dx = mBounds.mMax[0] - mBounds.mMin[0];
            RmReal dy = mBounds.mMax[1] - mBounds.mMin[1];
            RmReal dz = mBounds.mMax[2] - mBounds.mMin[2];

            AxisAABB axis  = AABB_XAXIS;
            RmReal   laxis = dx;

            if (dy > dx)
            {
                axis  = AABB_YAXIS;
                laxis = dy;
            }

            if (dz > dx && dz > dy)
            {
                axis  = AABB_ZAXIS;
                laxis = dz;
            }

            RmUint32 count = triangles.size();

            if (count < minLeafSize || depth >= maxDepth || laxis < minAxisSize)
            {

                mLeafTriangleIndex = leafTriangles.size(); 
                leafTriangles.push_back(count);
                for (TriVector::const_iterator i = triangles.begin(); i != triangles.end(); ++i)
                {
                    RmUint32 tri = *i;
                    leafTriangles.push_back(tri);
                }
            }
            else
            {
                RmReal center[3];
                mBounds.getCenter(center);
                BoundsAABB b1, b2;
                splitRect(axis, mBounds, b1, b2, center);

                BoundsAABB leftBounds, rightBounds;

                TriVector leftTriangles;
                TriVector rightTriangles;

                for (TriVector::const_iterator i = triangles.begin(); i != triangles.end(); ++i)
                {
                    RmUint32 tri = (*i);

                    {
                        RmUint32 i1 = indices[tri * 3 + 0];
                        RmUint32 i2 = indices[tri * 3 + 1];
                        RmUint32 i3 = indices[tri * 3 + 2];

                        const RmReal* p1 = &vertices[i1 * 3];
                        const RmReal* p2 = &vertices[i2 * 3];
                        const RmReal* p3 = &vertices[i3 * 3];

                        RmUint32 addCount = 0;
                        RmUint32 orCode   = 0xFFFFFFFF;
                        if (b1.containsTriangleExact(p1, p2, p3, orCode))
                        {
                            addCount++;
                            if (leftTriangles.empty())
                            {
                                leftBounds.setMin(p1);
                                leftBounds.setMax(p1);
                            }
                            leftBounds.include(p1);
                            leftBounds.include(p2);
                            leftBounds.include(p3);
                            leftTriangles.push_back(tri); 
                        }

                        if (orCode && b2.containsTriangleExact(p1, p2, p3, orCode))
                        {
                            addCount++;
                            if (rightTriangles.empty())
                            {
                                rightBounds.setMin(p1);
                                rightBounds.setMax(p1);
                            }
                            rightBounds.include(p1);
                            rightBounds.include(p2);
                            rightBounds.include(p3);
                            rightTriangles.push_back(tri); 
                        }
                        assert(addCount);
                    }
                }

                if (!leftTriangles.empty()) 
                {
                    leftBounds.clamp(b1);             
                    mLeft = callback->getNode();      
                    new (mLeft) NodeAABB(leftBounds); 

                    mLeft->split(leftTriangles, vcount, vertices, tcount, indices, depth + 1, maxDepth, minLeafSize, minAxisSize, callback, leafTriangles);
                }

                if (!rightTriangles.empty()) 
                {
                    rightBounds.clamp(b2);        
                    mRight = callback->getNode(); 
                    new (mRight) NodeAABB(rightBounds);

                    mRight->split(rightTriangles, vcount, vertices, tcount, indices, depth + 1, maxDepth, minLeafSize, minAxisSize, callback, leafTriangles);
                }
            }
        }

        void splitRect(AxisAABB axis, const BoundsAABB& source, BoundsAABB& b1, BoundsAABB& b2, const RmReal* midpoint)
        {
            switch (axis)
            {
                case AABB_XAXIS:
                {
                    b1.setMin(source.mMin);
                    b1.setMax(midpoint[0], source.mMax[1], source.mMax[2]);

                    b2.setMin(midpoint[0], source.mMin[1], source.mMin[2]);
                    b2.setMax(source.mMax);
                }
                break;
                case AABB_YAXIS:
                {
                    b1.setMin(source.mMin);
                    b1.setMax(source.mMax[0], midpoint[1], source.mMax[2]);

                    b2.setMin(source.mMin[0], midpoint[1], source.mMin[2]);
                    b2.setMax(source.mMax);
                }
                break;
                case AABB_ZAXIS:
                {
                    b1.setMin(source.mMin);
                    b1.setMax(source.mMax[0], source.mMax[1], midpoint[2]);

                    b2.setMin(source.mMin[0], source.mMin[1], midpoint[2]);
                    b2.setMax(source.mMax);
                }
                break;
            }
        }

        virtual void raycast(bool&            hit,
                             const RmReal*    from,
                             const RmReal*    to,
                             const RmReal*    dir,
                             RmReal*          hitLocation,
                             RmReal*          hitNormal,
                             RmReal*          hitDistance,
                             const RmReal*    vertices,
                             const RmUint32*  indices,
                             RmReal&          nearestDistance,
                             NodeInterface*   callback,
                             RmUint32*        raycastTriangles,
                             RmUint32         raycastFrame,
                             const TriVector& leafTriangles,
                             RmUint32&        nearestTriIndex)
        {
            RmReal sect[3];
            RmReal nd = nearestDistance;
            if (!intersectLineSegmentAABB(mBounds.mMin, mBounds.mMax, from, dir, nd, sect))
            {
                return;
            }
            if (mLeafTriangleIndex != TRI_EOF)
            {
                const RmUint32* scan  = &leafTriangles[mLeafTriangleIndex];
                RmUint32        count = *scan++;
                for (RmUint32 i = 0; i < count; i++)
                {
                    RmUint32 tri = *scan++;
                    if (raycastTriangles[tri] != raycastFrame)
                    {
                        raycastTriangles[tri] = raycastFrame;
                        RmUint32 i1           = indices[tri * 3 + 0];
                        RmUint32 i2           = indices[tri * 3 + 1];
                        RmUint32 i3           = indices[tri * 3 + 2];

                        const RmReal* p1 = &vertices[i1 * 3];
                        const RmReal* p2 = &vertices[i2 * 3];
                        const RmReal* p3 = &vertices[i3 * 3];

                        RmReal t;
                        if (rayIntersectsTriangle(from, dir, p1, p2, p3, t))
                        {
                            bool accept = false;
                            if (t == nearestDistance && tri < nearestTriIndex)
                            {
                                accept = true;
                            }
                            if (t < nearestDistance || accept)
                            {
                                nearestDistance = t;
                                if (hitLocation)
                                {
                                    hitLocation[0] = from[0] + dir[0] * t;
                                    hitLocation[1] = from[1] + dir[1] * t;
                                    hitLocation[2] = from[2] + dir[2] * t;
                                }
                                if (hitNormal)
                                {
                                    callback->getFaceNormal(tri, hitNormal);
                                }
                                if (hitDistance)
                                {
                                    *hitDistance = t;
                                }
                                nearestTriIndex = tri;
                                hit             = true;
                            }
                        }
                    }
                }
            }
            else
            {
                if (mLeft)
                {
                    mLeft->raycast(hit, from, to, dir, hitLocation, hitNormal, hitDistance, vertices, indices, nearestDistance, callback, raycastTriangles, raycastFrame, leafTriangles, nearestTriIndex);
                }
                if (mRight)
                {
                    mRight->raycast(hit, from, to, dir, hitLocation, hitNormal, hitDistance, vertices, indices, nearestDistance, callback, raycastTriangles, raycastFrame, leafTriangles, nearestTriIndex);
                }
            }
        }

        NodeAABB*  mLeft;              
        NodeAABB*  mRight;             
        BoundsAABB mBounds;            
        RmUint32   mLeafTriangleIndex; 
    };

    class MyRaycastMesh : public RaycastMesh, public NodeInterface
    {
    public:
        MyRaycastMesh(RmUint32 vcount, const RmReal* vertices, RmUint32 tcount, const RmUint32* indices, RmUint32 maxDepth, RmUint32 minLeafSize, RmReal minAxisSize)
        {
            mRaycastFrame = 0;
            if (maxDepth < 2)
            {
                maxDepth = 2;
            }
            if (maxDepth > 15)
            {
                maxDepth = 15;
            }
            RmUint32 pow2Table[16] = { 1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024, 2048, 4096, 8192, 16384, 65536 };
            mMaxNodeCount          = 0;
            for (RmUint32 i = 0; i <= maxDepth; i++)
            {
                mMaxNodeCount += pow2Table[i];
            }
            mNodes     = new NodeAABB[mMaxNodeCount];
            mNodeCount = 0;
            mVcount    = vcount;
            mVertices  = (RmReal*)::malloc(sizeof(RmReal) * 3 * vcount);
            memcpy(mVertices, vertices, sizeof(RmReal) * 3 * vcount);
            mTcount  = tcount;
            mIndices = (RmUint32*)::malloc(sizeof(RmUint32) * tcount * 3);
            memcpy(mIndices, indices, sizeof(RmUint32) * tcount * 3);
            mRaycastTriangles = (RmUint32*)::malloc(tcount * sizeof(RmUint32));
            memset(mRaycastTriangles, 0, tcount * sizeof(RmUint32));
            mRoot        = getNode();
            mFaceNormals = NULL;
            new (mRoot) NodeAABB(mVcount, mVertices, mTcount, mIndices, maxDepth, minLeafSize, minAxisSize, this, mLeafTriangles);
        }

        ~MyRaycastMesh(void)
        {
            delete[] mNodes;
            ::free(mVertices);
            ::free(mIndices);
            ::free(mFaceNormals);
            ::free(mRaycastTriangles);
        }

        virtual bool raycast(const RmReal* from, const RmReal* to, RmReal* hitLocation, RmReal* hitNormal, RmReal* hitDistance)
        {
            bool ret = false;

            RmReal dir[3];
            dir[0]          = to[0] - from[0];
            dir[1]          = to[1] - from[1];
            dir[2]          = to[2] - from[2];
            RmReal distance = sqrtf(dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2]);
            if (distance < 0.0000000001f)
                return false;
            RmReal recipDistance = 1.0f / distance;
            dir[0] *= recipDistance;
            dir[1] *= recipDistance;
            dir[2] *= recipDistance;
            mRaycastFrame++;
            RmUint32 nearestTriIndex = TRI_EOF;
            mRoot->raycast(ret, from, to, dir, hitLocation, hitNormal, hitDistance, mVertices, mIndices, distance, this, mRaycastTriangles, mRaycastFrame, mLeafTriangles, nearestTriIndex);
            return ret;
        }

        virtual void release(void)
        {
            delete this;
        }

        virtual const RmReal* getBoundMin(void) const 
        {
            return mRoot->mBounds.mMin;
        }
        virtual const RmReal* getBoundMax(void) const 
        {
            return mRoot->mBounds.mMax;
        }

        virtual NodeAABB* getNode(void)
        {
            assert(mNodeCount < mMaxNodeCount);
            NodeAABB* ret = &mNodes[mNodeCount];
            mNodeCount++;
            return ret;
        }

        virtual void getFaceNormal(RmUint32 tri, RmReal* faceNormal)
        {
            if (mFaceNormals == NULL)
            {
                mFaceNormals = (RmReal*)::malloc(sizeof(RmReal) * 3 * mTcount);
                for (RmUint32 i = 0; i < mTcount; i++)
                {
                    RmUint32      i1   = mIndices[i * 3 + 0];
                    RmUint32      i2   = mIndices[i * 3 + 1];
                    RmUint32      i3   = mIndices[i * 3 + 2];
                    const RmReal* p1   = &mVertices[i1 * 3];
                    const RmReal* p2   = &mVertices[i2 * 3];
                    const RmReal* p3   = &mVertices[i3 * 3];
                    RmReal*       dest = &mFaceNormals[i * 3];
                    computePlane(p3, p2, p1, dest);
                }
            }
            const RmReal* src = &mFaceNormals[tri * 3];
            faceNormal[0]     = src[0];
            faceNormal[1]     = src[1];
            faceNormal[2]     = src[2];
        }

        virtual bool bruteForceRaycast(const RmReal* from, const RmReal* to, RmReal* hitLocation, RmReal* hitNormal, RmReal* hitDistance)
        {
            bool ret = false;

            RmReal dir[3];

            dir[0] = to[0] - from[0];
            dir[1] = to[1] - from[1];
            dir[2] = to[2] - from[2];

            RmReal distance = sqrtf(dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2]);
            if (distance < 0.0000000001f)
                return false;
            RmReal recipDistance = 1.0f / distance;
            dir[0] *= recipDistance;
            dir[1] *= recipDistance;
            dir[2] *= recipDistance;
            const RmUint32* indices         = mIndices;
            const RmReal*   vertices        = mVertices;
            RmReal          nearestDistance = distance;

            for (RmUint32 tri = 0; tri < mTcount; tri++)
            {
                RmUint32 i1 = indices[tri * 3 + 0];
                RmUint32 i2 = indices[tri * 3 + 1];
                RmUint32 i3 = indices[tri * 3 + 2];

                const RmReal* p1 = &vertices[i1 * 3];
                const RmReal* p2 = &vertices[i2 * 3];
                const RmReal* p3 = &vertices[i3 * 3];

                RmReal t;
                if (rayIntersectsTriangle(from, dir, p1, p2, p3, t))
                {
                    if (t < nearestDistance)
                    {
                        nearestDistance = t;
                        if (hitLocation)
                        {
                            hitLocation[0] = from[0] + dir[0] * t;
                            hitLocation[1] = from[1] + dir[1] * t;
                            hitLocation[2] = from[2] + dir[2] * t;
                        }

                        if (hitNormal)
                        {
                            getFaceNormal(tri, hitNormal);
                        }

                        if (hitDistance)
                        {
                            *hitDistance = t;
                        }
                        ret = true;
                    }
                }
            }
            return ret;
        }

        RmUint32  mRaycastFrame;
        RmUint32* mRaycastTriangles;
        RmUint32  mVcount;
        RmReal*   mVertices;
        RmReal*   mFaceNormals;
        RmUint32  mTcount;
        RmUint32* mIndices;
        NodeAABB* mRoot;
        RmUint32  mNodeCount;
        RmUint32  mMaxNodeCount;
        NodeAABB* mNodes;
        TriVector mLeafTriangles;
    };

}; 

using namespace RAYCAST_MESH;

RaycastMesh* createRaycastMesh(RmUint32        vcount,      
                               const RmReal*   vertices,    
                               RmUint32        tcount,      
                               const RmUint32* indices,     
                               RmUint32        maxDepth,    
                               RmUint32        minLeafSize, 
                               RmReal          minAxisSize  
)
{
    MyRaycastMesh* m = new MyRaycastMesh(vcount, vertices, tcount, indices, maxDepth, minLeafSize, minAxisSize);
    return static_cast<RaycastMesh*>(m);
}
#include "RaycastMesh.h"
#include <assert.h>
#include <math.h>
#include <vector>


// This code snippet allows you to create an axis aligned bounding volume tree for a triangle mesh so that you can do
// high-speed raycasting.
//
// There are much better implementations of this available on the internet.  In particular I recommend that you use
// OPCODE written by Pierre Terdiman.
// @see: http://www.codercorner.com/Opcode.htm
//
// OPCODE does a whole lot more than just raycasting, and is a rather significant amount of source code.
//
// I am providing this code snippet for the use case where you *only* want to do quick and dirty optimized raycasting.
// I have not done performance testing between this version and OPCODE; so I don't know how much slower it is.  However,
// anytime you switch to using a spatial data structure for raycasting, you increase your performance by orders and orders
// of magnitude; so this implementation should work fine for simple tools and utilities.
//
// It also serves as a nice sample for people who are trying to learn the algorithm of how to implement AABB trees.
// AABB = Axis Aligned Bounding Volume trees.
//
// http://www.cgal.org/Manual/3.5/doc_html/cgal_manual/AABB_tree/Chapter_main.html
//
//
// This code snippet was written by John W. Ratcliff on August 18, 2011 and released under the MIT. license.
//
// mailto:jratcliffscarab@gmail.com
//
// The official source can be found at:  http://code.google.com/p/raycastmesh/
//
//

#pragma warning(disable : 4100)

namespace RAYCAST_MESH
{
    typedef std::vector<RmUint32> TriVector;

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
*	A method to compute a ray-AABB intersection.
*	Original code by Andrew Woo, from "Graphics Gems", Academic Press, 1990
*	Optimized code by Pierre Terdiman, 2000 (~20-30% faster on my Celeron 500)
*	Epsilon value added by Klaus Hartmann. (discarding it saves a few cycles only)
*
*	Hence this version is faster as well as more robust than the original one.
*
*	Should work provided:
*	1) the integer representation of 0.0f is 0x00000000
*	2) the sign bit of the RmReal is the most significant one
*
*	Report bugs: p.terdiman@codercorner.com
*
*	\param		aabb		[in] the axis-aligned bounding box
*	\param		origin		[in] ray origin
*	\param		dir			[in] ray direction
*	\param		coord		[out] impact coordinates
*	\return		true if ray intersects AABB
*/
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
#define RAYAABB_EPSILON 0.00001f
//! Integer representation of a RmRealing-point value.
#define IR(x) ((RmUint32&)x)

    bool intersectRayAABB(const RmReal MinB[3], const RmReal MaxB[3], const RmReal origin[3], const RmReal dir[3], RmReal coord[3])
    {
        bool   Inside = true;
        RmReal MaxT[3];
        MaxT[0] = MaxT[1] = MaxT[2] = -1.0f;

        // Find candidate planes.
        for (RmUint32 i = 0; i < 3; i++)
        {
            if (origin[i] < MinB[i])
            {
                coord[i] = MinB[i];
                Inside   = false;

                // Calculate T distances to candidate planes
                if (IR(dir[i]))
                    MaxT[i] = (MinB[i] - origin[i]) / dir[i];
            }
            else if (origin[i] > MaxB[i])
            {
                coord[i] = MaxB[i];
                Inside   = false;

                // Calculate T distances to candidate planes
                if (IR(dir[i]))
                    MaxT[i] = (MaxB[i] - origin[i]) / dir[i];
            }
        }

        // Ray origin inside bounding box
        if (Inside)
        {
            coord[0] = origin[0];
            coord[1] = origin[1];
            coord[2] = origin[2];
            return true;
        }

        // Get largest of the maxT's for final choice of intersection
        RmUint32 WhichPlane = 0;
        if (MaxT[1] > MaxT[WhichPlane])
            WhichPlane = 1;
        if (MaxT[2] > MaxT[WhichPlane])
            WhichPlane = 2;

        // Check final candidate actually inside box
        if (IR(MaxT[WhichPlane]) & 0x80000000)
            return false;

        for (RmUint32 i = 0; i < 3; i++)
        {
            if (i != WhichPlane)
            {
                coord[i] = origin[i] + MaxT[WhichPlane] * dir[i];
#ifdef RAYAABB_EPSILON
                if (coord[i] < MinB[i] - RAYAABB_EPSILON || coord[i] > MaxB[i] + RAYAABB_EPSILON)
                    return false;
#else
                if (coord[i] < MinB[i] || coord[i] > MaxB[i])
                    return false;
#endif
            }
        }
        return true; // ray hits box
    }

    bool intersectLineSegmentAABB(const RmReal bmin[3], const RmReal bmax[3], const RmReal p1[3], const RmReal dir[3], RmReal& dist, RmReal intersect[3])
    {
        bool ret = false;

        if (dist > RAYAABB_EPSILON)
        {
            ret = intersectRayAABB(bmin, bmax, p1, dir, intersect);
            if (ret)
            {
                RmReal dx = p1[0] - intersect[0];
                RmReal dy = p1[1] - intersect[1];
                RmReal dz = p1[2] - intersect[2];
                RmReal d  = dx * dx + dy * dy + dz * dz;
                if (d < dist * dist)
                {
                    dist = sqrtf(d);
                }
                else
                {
                    ret = false;
                }
            }
        }
        return ret;
    }

/* a = b - c */
#define vector(a, b, c)       \
    (a)[0] = (b)[0] - (c)[0]; \
    (a)[1] = (b)[1] - (c)[1]; \
    (a)[2] = (b)[2] - (c)[2];

#define innerProduct(v, q) \
    ((v)[0] * (q)[0] +     \
     (v)[1] * (q)[1] +     \
     (v)[2] * (q)[2])

#define crossProduct(a, b, c)                   \
    (a)[0] = (b)[1] * (c)[2] - (c)[1] * (b)[2]; \
    (a)[1] = (b)[2] * (c)[0] - (c)[2] * (b)[0]; \
    (a)[2] = (b)[0] * (c)[1] - (c)[0] * (b)[1];

    static inline bool rayIntersectsTriangle(const RmReal* p, const RmReal* d, const RmReal* v0, const RmReal* v1, const RmReal* v2, RmReal& t)
    {
        RmReal e1[3], e2[3], h[3], s[3], q[3];
        RmReal a, f, u, v;

        vector(e1, v1, v0);
        vector(e2, v2, v0);
        crossProduct(h, d, e2);
        a = innerProduct(e1, h);

        if (a > -0.00001 && a < 0.00001)
            return (false);

        f = 1 / a;
        vector(s, p, v0);
        u = f * (innerProduct(s, h));

        if (u < 0.0 || u > 1.0)
            return (false);

        crossProduct(q, s, e1);
        v = f * innerProduct(d, q);
        if (v < 0.0 || u + v > 1.0)
            return (false);
        // at this stage we can compute t to find out where
        // the intersection point is on the line
        t = f * innerProduct(e2, q);
        if (t > 0) // ray intersection
            return (true);
        else // this means that there is a line intersection
            // but not a ray intersection
            return (false);
    }

    static RmReal computePlane(const RmReal* A, const RmReal* B, const RmReal* C, RmReal* n) // returns D
    {
        RmReal vx = (B[0] - C[0]);
        RmReal vy = (B[1] - C[1]);
        RmReal vz = (B[2] - C[2]);

        RmReal wx = (A[0] - B[0]);
        RmReal wy = (A[1] - B[1]);
        RmReal wz = (A[2] - B[2]);

        RmReal vw_x = vy * wz - vz * wy;
        RmReal vw_y = vz * wx - vx * wz;
        RmReal vw_z = vx * wy - vy * wx;

        RmReal mag = sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));

        if (mag < 0.000001f)
        {
            mag = 0;
        }
        else
        {
            mag = 1.0f / mag;
        }

        RmReal x = vw_x * mag;
        RmReal y = vw_y * mag;
        RmReal z = vw_z * mag;

        RmReal D = 0.0f - ((x * A[0]) + (y * A[1]) + (z * A[2]));

        n[0] = x;
        n[1] = y;
        n[2] = z;

        return D;
    }

#define TRI_EOF 0xFFFFFFFF

    enum AxisAABB
    {
        AABB_XAXIS,
        AABB_YAXIS,
        AABB_ZAXIS
    };

    enum ClipCode
    {
        CC_MINX = (1 << 0),
        CC_MAXX = (1 << 1),
        CC_MINY = (1 << 2),
        CC_MAXY = (1 << 3),
        CC_MINZ = (1 << 4),
        CC_MAXZ = (1 << 5),
    };

    class BoundsAABB
    {
    public:
        void setMin(const RmReal* v)
        {
            mMin[0] = v[0];
            mMin[1] = v[1];
            mMin[2] = v[2];
        }

        void setMax(const RmReal* v)
        {
            mMax[0] = v[0];
            mMax[1] = v[1];
            mMax[2] = v[2];
        }

        void setMin(RmReal x, RmReal y, RmReal z)
        {
            mMin[0] = x;
            mMin[1] = y;
            mMin[2] = z;
        }

        void setMax(RmReal x, RmReal y, RmReal z)
        {
            mMax[0] = x;
            mMax[1] = y;
            mMax[2] = z;
        }

        void include(const RmReal* v)
        {
            if (v[0] < mMin[0])
                mMin[0] = v[0];
            if (v[1] < mMin[1])
                mMin[1] = v[1];
            if (v[2] < mMin[2])
                mMin[2] = v[2];

            if (v[0] > mMax[0])
                mMax[0] = v[0];
            if (v[1] > mMax[1])
                mMax[1] = v[1];
            if (v[2] > mMax[2])
                mMax[2] = v[2];
        }

        void getCenter(RmReal* center) const
        {
            center[0] = (mMin[0] + mMax[0]) * 0.5f;
            center[1] = (mMin[1] + mMax[1]) * 0.5f;
            center[2] = (mMin[2] + mMax[2]) * 0.5f;
        }

        bool intersects(const BoundsAABB& b) const
        {
            if ((mMin[0] > b.mMax[0]) || (b.mMin[0] > mMax[0]))
                return false;
            if ((mMin[1] > b.mMax[1]) || (b.mMin[1] > mMax[1]))
                return false;
            if ((mMin[2] > b.mMax[2]) || (b.mMin[2] > mMax[2]))
                return false;
            return true;
        }

        bool containsTriangle(const RmReal* p1, const RmReal* p2, const RmReal* p3) const
        {
            BoundsAABB b;
            b.setMin(p1);
            b.setMax(p1);
            b.include(p2);
            b.include(p3);
            return intersects(b);
        }

        bool containsTriangleExact(const RmReal* p1, const RmReal* p2, const RmReal* p3, RmUint32& orCode) const
        {
            bool ret = false;

            RmUint32 andCode;
            orCode = getClipCode(p1, p2, p3, andCode);
            if (andCode == 0)
            {
                ret = true;
            }

            return ret;
        }

        inline RmUint32 getClipCode(const RmReal* p1, const RmReal* p2, const RmReal* p3, RmUint32& andCode) const
        {
            andCode     = 0xFFFFFFFF;
            RmUint32 c1 = getClipCode(p1);
            RmUint32 c2 = getClipCode(p2);
            RmUint32 c3 = getClipCode(p3);
            andCode &= c1;
            andCode &= c2;
            andCode &= c3;
            return c1 | c2 | c3;
        }

        inline RmUint32 getClipCode(const RmReal* p) const
        {
            RmUint32 ret = 0;

            if (p[0] < mMin[0])
            {
                ret |= CC_MINX;
            }
            else if (p[0] > mMax[0])
            {
                ret |= CC_MAXX;
            }

            if (p[1] < mMin[1])
            {
                ret |= CC_MINY;
            }
            else if (p[1] > mMax[1])
            {
                ret |= CC_MAXY;
            }

            if (p[2] < mMin[2])
            {
                ret |= CC_MINZ;
            }
            else if (p[2] > mMax[2])
            {
                ret |= CC_MAXZ;
            }

            return ret;
        }

        inline void clamp(const BoundsAABB& aabb)
        {
            if (mMin[0] < aabb.mMin[0])
                mMin[0] = aabb.mMin[0];
            if (mMin[1] < aabb.mMin[1])
                mMin[1] = aabb.mMin[1];
            if (mMin[2] < aabb.mMin[2])
                mMin[2] = aabb.mMin[2];
            if (mMax[0] > aabb.mMax[0])
                mMax[0] = aabb.mMax[0];
            if (mMax[1] > aabb.mMax[1])
                mMax[1] = aabb.mMax[1];
            if (mMax[2] > aabb.mMax[2])
                mMax[2] = aabb.mMax[2];
        }

        RmReal mMin[3];
        RmReal mMax[3];
    };

    class NodeAABB;

    class NodeInterface
    {
    public:
        virtual NodeAABB* getNode(void)                                   = 0;
        virtual void      getFaceNormal(RmUint32 tri, RmReal* faceNormal) = 0;
    };

    class NodeAABB
    {
    public:
        NodeAABB(void)
        {
            mLeft              = NULL;
            mRight             = NULL;
            mLeafTriangleIndex = TRI_EOF;
        }

        NodeAABB(RmUint32 vcount, const RmReal* vertices, RmUint32 tcount, RmUint32* indices,
                 RmUint32       maxDepth,    // Maximum recursion depth for the triangle mesh.
                 RmUint32       minLeafSize, // minimum triangles to treat as a 'leaf' node.
                 RmReal         minAxisSize,
                 NodeInterface* callback,
                 TriVector&     leafTriangles) // once a particular axis is less than this size, stop sub-dividing.

        {
            mLeft              = NULL;
            mRight             = NULL;
            mLeafTriangleIndex = TRI_EOF;
            TriVector triangles;
            triangles.reserve(tcount);
            for (RmUint32 i = 0; i < tcount; i++)
            {
                triangles.push_back(i);
            }
            mBounds.setMin(vertices);
            mBounds.setMax(vertices);
            const RmReal* vtx = vertices + 3;
            for (RmUint32 i = 1; i < vcount; i++)
            {
                mBounds.include(vtx);
                vtx += 3;
            }
            split(triangles, vcount, vertices, tcount, indices, 0, maxDepth, minLeafSize, minAxisSize, callback, leafTriangles);
        }

        NodeAABB(const BoundsAABB& aabb)
        {
            mBounds            = aabb;
            mLeft              = NULL;
            mRight             = NULL;
            mLeafTriangleIndex = TRI_EOF;
        }

        ~NodeAABB(void)
        {
        }

        // here is where we split the mesh..
        void split(const TriVector& triangles,
                   RmUint32         vcount,
                   const RmReal*    vertices,
                   RmUint32         tcount,
                   const RmUint32*  indices,
                   RmUint32         depth,
                   RmUint32         maxDepth,    // Maximum recursion depth for the triangle mesh.
                   RmUint32         minLeafSize, // minimum triangles to treat as a 'leaf' node.
                   RmReal           minAxisSize,
                   NodeInterface*   callback,
                   TriVector&       leafTriangles) // once a particular axis is less than this size, stop sub-dividing.

        {
            // Find the longest axis of the bounding volume of this node
            RmReal dx = mBounds.mMax[0] - mBounds.mMin[0];
            RmReal dy = mBounds.mMax[1] - mBounds.mMin[1];
            RmReal dz = mBounds.mMax[2] - mBounds.mMin[2];

            AxisAABB axis  = AABB_XAXIS;
            RmReal   laxis = dx;

            if (dy > dx)
            {
                axis  = AABB_YAXIS;
                laxis = dy;
            }

            if (dz > dx && dz > dy)
            {
                axis  = AABB_ZAXIS;
                laxis = dz;
            }

            RmUint32 count = triangles.size();

            // if the number of triangles is less than the minimum allowed for a leaf node or...
            // we have reached the maximum recursion depth or..
            // the width of the longest axis is less than the minimum axis size then...
            // we create the leaf node and copy the triangles into the leaf node triangle array.
            if (count < minLeafSize || depth >= maxDepth || laxis < minAxisSize)
            {
                // Copy the triangle indices into the leaf triangles array
                mLeafTriangleIndex = leafTriangles.size(); // assign the array start location for these leaf triangles.
                leafTriangles.push_back(count);
                for (TriVector::const_iterator i = triangles.begin(); i != triangles.end(); ++i)
                {
                    RmUint32 tri = *i;
                    leafTriangles.push_back(tri);
                }
            }
            else
            {
                RmReal center[3];
                mBounds.getCenter(center);
                BoundsAABB b1, b2;
                splitRect(axis, mBounds, b1, b2, center);

                // Compute two bounding boxes based upon the split of the longest axis

                BoundsAABB leftBounds, rightBounds;

                TriVector leftTriangles;
                TriVector rightTriangles;

                // Create two arrays; one of all triangles which intersect the 'left' half of the bounding volume node
                // and another array that includes all triangles which intersect the 'right' half of the bounding volume node.
                for (TriVector::const_iterator i = triangles.begin(); i != triangles.end(); ++i)
                {
                    RmUint32 tri = (*i);

                    {
                        RmUint32 i1 = indices[tri * 3 + 0];
                        RmUint32 i2 = indices[tri * 3 + 1];
                        RmUint32 i3 = indices[tri * 3 + 2];

                        const RmReal* p1 = &vertices[i1 * 3];
                        const RmReal* p2 = &vertices[i2 * 3];
                        const RmReal* p3 = &vertices[i3 * 3];

                        RmUint32 addCount = 0;
                        RmUint32 orCode   = 0xFFFFFFFF;
                        if (b1.containsTriangleExact(p1, p2, p3, orCode))
                        {
                            addCount++;
                            if (leftTriangles.empty())
                            {
                                leftBounds.setMin(p1);
                                leftBounds.setMax(p1);
                            }
                            leftBounds.include(p1);
                            leftBounds.include(p2);
                            leftBounds.include(p3);
                            leftTriangles.push_back(tri); // Add this triangle to the 'left triangles' array and revise the left triangles bounding volume
                        }
                        // if the orCode is zero; meaning the triangle was fully self-contiained int he left bounding box; then we don't need to test against the right
                        if (orCode && b2.containsTriangleExact(p1, p2, p3, orCode))
                        {
                            addCount++;
                            if (rightTriangles.empty())
                            {
                                rightBounds.setMin(p1);
                                rightBounds.setMax(p1);
                            }
                            rightBounds.include(p1);
                            rightBounds.include(p2);
                            rightBounds.include(p3);
                            rightTriangles.push_back(tri); // Add this triangle to the 'right triangles' array and revise the right triangles bounding volume.
                        }
                        assert(addCount);
                    }
                }

                if (!leftTriangles.empty()) // If there are triangles in the left half then...
                {
                    leftBounds.clamp(b1);             // we have to clamp the bounding volume so it stays inside the parent volume.
                    mLeft = callback->getNode();      // get a new AABB node
                    new (mLeft) NodeAABB(leftBounds); // initialize it to default constructor values.
                    // Then recursively split this node.
                    mLeft->split(leftTriangles, vcount, vertices, tcount, indices, depth + 1, maxDepth, minLeafSize, minAxisSize, callback, leafTriangles);
                }

                if (!rightTriangles.empty()) // If there are triangles in the right half then..
                {
                    rightBounds.clamp(b2);        // clamps the bounding volume so it stays restricted to the size of the parent volume.
                    mRight = callback->getNode(); // allocate and default initialize a new node
                    new (mRight) NodeAABB(rightBounds);
                    // Recursively split this node.
                    mRight->split(rightTriangles, vcount, vertices, tcount, indices, depth + 1, maxDepth, minLeafSize, minAxisSize, callback, leafTriangles);
                }
            }
        }

        void splitRect(AxisAABB axis, const BoundsAABB& source, BoundsAABB& b1, BoundsAABB& b2, const RmReal* midpoint)
        {
            switch (axis)
            {
                case AABB_XAXIS:
                {
                    b1.setMin(source.mMin);
                    b1.setMax(midpoint[0], source.mMax[1], source.mMax[2]);

                    b2.setMin(midpoint[0], source.mMin[1], source.mMin[2]);
                    b2.setMax(source.mMax);
                }
                break;
                case AABB_YAXIS:
                {
                    b1.setMin(source.mMin);
                    b1.setMax(source.mMax[0], midpoint[1], source.mMax[2]);

                    b2.setMin(source.mMin[0], midpoint[1], source.mMin[2]);
                    b2.setMax(source.mMax);
                }
                break;
                case AABB_ZAXIS:
                {
                    b1.setMin(source.mMin);
                    b1.setMax(source.mMax[0], source.mMax[1], midpoint[2]);

                    b2.setMin(source.mMin[0], source.mMin[1], midpoint[2]);
                    b2.setMax(source.mMax);
                }
                break;
            }
        }

        virtual void raycast(bool&            hit,
                             const RmReal*    from,
                             const RmReal*    to,
                             const RmReal*    dir,
                             RmReal*          hitLocation,
                             RmReal*          hitNormal,
                             RmReal*          hitDistance,
                             const RmReal*    vertices,
                             const RmUint32*  indices,
                             RmReal&          nearestDistance,
                             NodeInterface*   callback,
                             RmUint32*        raycastTriangles,
                             RmUint32         raycastFrame,
                             const TriVector& leafTriangles,
                             RmUint32&        nearestTriIndex)
        {
            RmReal sect[3];
            RmReal nd = nearestDistance;
            if (!intersectLineSegmentAABB(mBounds.mMin, mBounds.mMax, from, dir, nd, sect))
            {
                return;
            }
            if (mLeafTriangleIndex != TRI_EOF)
            {
                const RmUint32* scan  = &leafTriangles[mLeafTriangleIndex];
                RmUint32        count = *scan++;
                for (RmUint32 i = 0; i < count; i++)
                {
                    RmUint32 tri = *scan++;
                    if (raycastTriangles[tri] != raycastFrame)
                    {
                        raycastTriangles[tri] = raycastFrame;
                        RmUint32 i1           = indices[tri * 3 + 0];
                        RmUint32 i2           = indices[tri * 3 + 1];
                        RmUint32 i3           = indices[tri * 3 + 2];

                        const RmReal* p1 = &vertices[i1 * 3];
                        const RmReal* p2 = &vertices[i2 * 3];
                        const RmReal* p3 = &vertices[i3 * 3];

                        RmReal t;
                        if (rayIntersectsTriangle(from, dir, p1, p2, p3, t))
                        {
                            bool accept = false;
                            if (t == nearestDistance && tri < nearestTriIndex)
                            {
                                accept = true;
                            }
                            if (t < nearestDistance || accept)
                            {
                                nearestDistance = t;
                                if (hitLocation)
                                {
                                    hitLocation[0] = from[0] + dir[0] * t;
                                    hitLocation[1] = from[1] + dir[1] * t;
                                    hitLocation[2] = from[2] + dir[2] * t;
                                }
                                if (hitNormal)
                                {
                                    callback->getFaceNormal(tri, hitNormal);
                                }
                                if (hitDistance)
                                {
                                    *hitDistance = t;
                                }
                                nearestTriIndex = tri;
                                hit             = true;
                            }
                        }
                    }
                }
            }
            else
            {
                if (mLeft)
                {
                    mLeft->raycast(hit, from, to, dir, hitLocation, hitNormal, hitDistance, vertices, indices, nearestDistance, callback, raycastTriangles, raycastFrame, leafTriangles, nearestTriIndex);
                }
                if (mRight)
                {
                    mRight->raycast(hit, from, to, dir, hitLocation, hitNormal, hitDistance, vertices, indices, nearestDistance, callback, raycastTriangles, raycastFrame, leafTriangles, nearestTriIndex);
                }
            }
        }

        NodeAABB*  mLeft;              // left node
        NodeAABB*  mRight;             // right node
        BoundsAABB mBounds;            // bounding volume of node
        RmUint32   mLeafTriangleIndex; // if it is a leaf node; then these are the triangle indices.
    };

    class MyRaycastMesh : public RaycastMesh, public NodeInterface
    {
    public:
        MyRaycastMesh(RmUint32 vcount, const RmReal* vertices, RmUint32 tcount, const RmUint32* indices, RmUint32 maxDepth, RmUint32 minLeafSize, RmReal minAxisSize)
        {
            mRaycastFrame = 0;
            if (maxDepth < 2)
            {
                maxDepth = 2;
            }
            if (maxDepth > 15)
            {
                maxDepth = 15;
            }
            RmUint32 pow2Table[16] = { 1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024, 2048, 4096, 8192, 16384, 65536 };
            mMaxNodeCount          = 0;
            for (RmUint32 i = 0; i <= maxDepth; i++)
            {
                mMaxNodeCount += pow2Table[i];
            }
            mNodes     = new NodeAABB[mMaxNodeCount];
            mNodeCount = 0;
            mVcount    = vcount;
            mVertices  = (RmReal*)::malloc(sizeof(RmReal) * 3 * vcount);
            memcpy(mVertices, vertices, sizeof(RmReal) * 3 * vcount);
            mTcount  = tcount;
            mIndices = (RmUint32*)::malloc(sizeof(RmUint32) * tcount * 3);
            memcpy(mIndices, indices, sizeof(RmUint32) * tcount * 3);
            mRaycastTriangles = (RmUint32*)::malloc(tcount * sizeof(RmUint32));
            memset(mRaycastTriangles, 0, tcount * sizeof(RmUint32));
            mRoot        = getNode();
            mFaceNormals = NULL;
            new (mRoot) NodeAABB(mVcount, mVertices, mTcount, mIndices, maxDepth, minLeafSize, minAxisSize, this, mLeafTriangles);
        }

        ~MyRaycastMesh(void)
        {
            delete[] mNodes;
            ::free(mVertices);
            ::free(mIndices);
            ::free(mFaceNormals);
            ::free(mRaycastTriangles);
        }

        virtual bool raycast(const RmReal* from, const RmReal* to, RmReal* hitLocation, RmReal* hitNormal, RmReal* hitDistance)
        {
            bool ret = false;

            RmReal dir[3];
            dir[0]          = to[0] - from[0];
            dir[1]          = to[1] - from[1];
            dir[2]          = to[2] - from[2];
            RmReal distance = sqrtf(dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2]);
            if (distance < 0.0000000001f)
                return false;
            RmReal recipDistance = 1.0f / distance;
            dir[0] *= recipDistance;
            dir[1] *= recipDistance;
            dir[2] *= recipDistance;
            mRaycastFrame++;
            RmUint32 nearestTriIndex = TRI_EOF;
            mRoot->raycast(ret, from, to, dir, hitLocation, hitNormal, hitDistance, mVertices, mIndices, distance, this, mRaycastTriangles, mRaycastFrame, mLeafTriangles, nearestTriIndex);
            return ret;
        }

        virtual void release(void)
        {
            delete this;
        }

        virtual const RmReal* getBoundMin(void) const // return the minimum bounding box
        {
            return mRoot->mBounds.mMin;
        }
        virtual const RmReal* getBoundMax(void) const // return the maximum bounding box.
        {
            return mRoot->mBounds.mMax;
        }

        virtual NodeAABB* getNode(void)
        {
            assert(mNodeCount < mMaxNodeCount);
            NodeAABB* ret = &mNodes[mNodeCount];
            mNodeCount++;
            return ret;
        }

        virtual void getFaceNormal(RmUint32 tri, RmReal* faceNormal)
        {
            if (mFaceNormals == NULL)
            {
                mFaceNormals = (RmReal*)::malloc(sizeof(RmReal) * 3 * mTcount);
                for (RmUint32 i = 0; i < mTcount; i++)
                {
                    RmUint32      i1   = mIndices[i * 3 + 0];
                    RmUint32      i2   = mIndices[i * 3 + 1];
                    RmUint32      i3   = mIndices[i * 3 + 2];
                    const RmReal* p1   = &mVertices[i1 * 3];
                    const RmReal* p2   = &mVertices[i2 * 3];
                    const RmReal* p3   = &mVertices[i3 * 3];
                    RmReal*       dest = &mFaceNormals[i * 3];
                    computePlane(p3, p2, p1, dest);
                }
            }
            const RmReal* src = &mFaceNormals[tri * 3];
            faceNormal[0]     = src[0];
            faceNormal[1]     = src[1];
            faceNormal[2]     = src[2];
        }

        virtual bool bruteForceRaycast(const RmReal* from, const RmReal* to, RmReal* hitLocation, RmReal* hitNormal, RmReal* hitDistance)
        {
            bool ret = false;

            RmReal dir[3];

            dir[0] = to[0] - from[0];
            dir[1] = to[1] - from[1];
            dir[2] = to[2] - from[2];

            RmReal distance = sqrtf(dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2]);
            if (distance < 0.0000000001f)
                return false;
            RmReal recipDistance = 1.0f / distance;
            dir[0] *= recipDistance;
            dir[1] *= recipDistance;
            dir[2] *= recipDistance;
            const RmUint32* indices         = mIndices;
            const RmReal*   vertices        = mVertices;
            RmReal          nearestDistance = distance;

            for (RmUint32 tri = 0; tri < mTcount; tri++)
            {
                RmUint32 i1 = indices[tri * 3 + 0];
                RmUint32 i2 = indices[tri * 3 + 1];
                RmUint32 i3 = indices[tri * 3 + 2];

                const RmReal* p1 = &vertices[i1 * 3];
                const RmReal* p2 = &vertices[i2 * 3];
                const RmReal* p3 = &vertices[i3 * 3];

                RmReal t;
                if (rayIntersectsTriangle(from, dir, p1, p2, p3, t))
                {
                    if (t < nearestDistance)
                    {
                        nearestDistance = t;
                        if (hitLocation)
                        {
                            hitLocation[0] = from[0] + dir[0] * t;
                            hitLocation[1] = from[1] + dir[1] * t;
                            hitLocation[2] = from[2] + dir[2] * t;
                        }

                        if (hitNormal)
                        {
                            getFaceNormal(tri, hitNormal);
                        }

                        if (hitDistance)
                        {
                            *hitDistance = t;
                        }
                        ret = true;
                    }
                }
            }
            return ret;
        }

        RmUint32  mRaycastFrame;
        RmUint32* mRaycastTriangles;
        RmUint32  mVcount;
        RmReal*   mVertices;
        RmReal*   mFaceNormals;
        RmUint32  mTcount;
        RmUint32* mIndices;
        NodeAABB* mRoot;
        RmUint32  mNodeCount;
        RmUint32  mMaxNodeCount;
        NodeAABB* mNodes;
        TriVector mLeafTriangles;
    };

}; // namespace RAYCAST_MESH

using namespace RAYCAST_MESH;

RaycastMesh* createRaycastMesh(RmUint32        vcount,      // The number of vertices in the source triangle mesh
                               const RmReal*   vertices,    // The array of vertex positions in the format x1,y1,z1..x2,y2,z2.. etc.
                               RmUint32        tcount,      // The number of triangles in the source triangle mesh
                               const RmUint32* indices,     // The triangle indices in the format of i1,i2,i3 ... i4,i5,i6, ...
                               RmUint32        maxDepth,    // Maximum recursion depth for the triangle mesh.
                               RmUint32        minLeafSize, // minimum triangles to treat as a 'leaf' node.
                               RmReal          minAxisSize  // once a particular axis is less than this size, stop sub-dividing.
)
{
    MyRaycastMesh* m = new MyRaycastMesh(vcount, vertices, tcount, indices, maxDepth, minLeafSize, minAxisSize);
    return static_cast<RaycastMesh*>(m);
}
