/**
  @file CollisionDetection.cpp
  
  @maintainer Morgan McGuire, http://graphics.cs.williams.edu

  @cite Bounce direction based on Paul Nettle's ftp://ftp.3dmaileffects.com/pub/FluidStudios/CollisionDetection/Fluid_Studios_Generic_Collision_Detection_for_Games_Using_Ellipsoids.pdf and comments by Max McGuire.  Ray-sphere code by Eric Haines.

  @created 2001-11-24
  @edited  2008-12-29
 */

#include "G3D/CoordinateFrame.h"
#include "G3D/platform.h"
#include "G3D/CollisionDetection.h"
#include "G3D/debugAssert.h"
#include "G3D/vectorMath.h"
#include "G3D/Capsule.h"
#include "G3D/Plane.h"
#include "G3D/Line.h"
#include "G3D/LineSegment.h"
#include "G3D/Sphere.h"
#include "G3D/Box.h"
#include "G3D/Triangle.h"
#include "G3D/Vector3.h"
#include "G3D/AABox.h"

#ifdef _MSC_VER
// Turn on fast floating-point optimizations
#pragma float_control( push )
#pragma fp_contract( on ) 
#pragma fenv_access( off )
#pragma float_control( except, off )
#pragma float_control( precise, off )
#endif


namespace G3D {

bool CollisionDetection::ignoreBool;
Vector3    CollisionDetection::ignore;
Array<Vector3> CollisionDetection::ignoreArray;



Vector3 CollisionDetection::separatingAxisForSolidBoxSolidBox(
        const int separatingAxisIndex,
        const Box & box1,
        const Box & box2) {
    debugAssert(separatingAxisIndex >= 0);
    debugAssert(separatingAxisIndex < 15);
    Vector3 axis;
    if (separatingAxisIndex < 3) {
        axis = box1.axis(separatingAxisIndex);
    } else if (separatingAxisIndex < 6) {
        axis = box2.axis(separatingAxisIndex - 3);
    } else {
        int box1Index = (separatingAxisIndex - 6) / 3;
        int box2Index = (separatingAxisIndex - 6) % 3;
        axis = cross(box1.axis(box1Index), box2.axis(box2Index));
    }
    return axis;
}

#ifdef _MSC_VER
#   pragma warning (push)
#   pragma warning (disable : 4244)
#endif

float CollisionDetection::projectedDistanceForSolidBoxSolidBox(
    const int           separatingAxisIndex,
    const Vector3 &     a,
    const Vector3 &     b,
    const Vector3 &     D,
    const double*       c,
    const double*       ca,
    const double*       ad,
    const double*       bd)
{
    (void)D;

    float R0 = 0.0f;
    float R1 = 0.0f;
    float R = 0.0f;
    switch (separatingAxisIndex) {
    case 0:
        // A0
        R0 = a[0];
        R1 = b[0] * ca[0] + b[1] * ca[1] + b[2] * ca[2];
        R = fabs(ad[0]);
        break;
    case 1:
        // A1
        R0 = a[1];
        R1 = b[0] * ca[3] + b[1] * ca[4] + b[2] * ca[5];
        R = fabs(ad[1]);
        break;
    case 2:
        // A2
        R0 = a[2];
        R1 = b[0] * ca[6] + b[1] * ca[7] + b[2] * ca[8];
        R = fabs(ad[2]);
        break;
    case 3:
        // B0
        R0 = a[0] * ca[0] + a[1] * ca[3] + a[2] * ca[6];
        R1 = b[0];
        R = fabs(bd[0]);
        break;
    case 4:
        // B1
        R0 = a[0] * ca[1] + a[1] * ca[4] + a[2] * ca[7];
        R1 = b[1];
        R = fabs(bd[1]);
        break;
    case 5:
        // B2
        R0 = a[0] * ca[2] + a[1] * ca[5] + a[2] * ca[8];
        R1 = b[2];
        R = fabs(bd[2]);
        break;
    case 6:
        // A0 x B0
        R0 = a[1] * ca[6] + a[2] * ca[3];
        R1 = b[1] * ca[2] + b[2] * ca[1];
        R = fabs(c[3] * ad[2] - c[6] * ad[1]);
        break;
    case 7:
        // A0 x B1
        R0 = a[1] * ca[7] + a[2] * ca[4];
        R1 = b[0] * ca[2] + b[2] * ca[0];
        R = fabs(c[4] * ad[2] - c[7] * ad[1]);
        break;
    case 8:
        // A0 x B2
        R0 = a[1] * ca[8] + a[2] * ca[5];
        R1 = b[0] * ca[1] + b[1] * ca[0];
        R = fabs(c[5] * ad[2] - c[8] * ad[1]);
        break;
    case 9:
        // A1 x B0
        R0 = a[0] * ca[6] + a[2] * ca[0];
        R1 = b[1] * ca[5] + b[2] * ca[4];
        R = fabs(c[6] * ad[0] - c[0] * ad[2]);
        break;
    case 10:
        // A1 x B1
        R0 = a[0] * ca[7] + a[2] * ca[1];
        R1 = b[0] * ca[5] + b[2] * ca[3];
        R = fabs(c[7] * ad[0] - c[1] * ad[2]);
        break;
    case 11:
        // A1 x B2
        R0 = a[0] * ca[8] + a[2] * ca[2];
        R1 = b[0] * ca[4] + b[1] * ca[3];
        R = fabs(c[8] * ad[0] - c[2] * ad[2]);
        break;
    case 12:
        // A2 x B0
        R0 = a[0] * ca[3] + a[1] * ca[0];
        R1 = b[1] * ca[8] + b[2] * ca[7];
        R = fabs(c[0] * ad[1] - c[3] * ad[0]);
        break;
    case 13:
        // A2 x B1
        R0 = a[0] * ca[4] + a[1] * ca[1];
        R1 = b[0] * ca[8] + b[2] * ca[6];
        R = fabs(c[1] * ad[1] - c[4] * ad[0]);
        break;
    case 14:
        // A2 x B2
        R0 = a[0] * ca[5] + a[1] * ca[2];
        R1 = b[0] * ca[7] + b[1] * ca[6];
        R = fabs(c[2] * ad[1] - c[5] * ad[0]);
        break;
    default:
        debugAssertM(false, "fell through switch statement");
    }

    return (R - (R0 + R1));
}


bool CollisionDetection::parallelAxisForSolidBoxSolidBox(
        const double* ca,
        const double epsilon,
        int & axis1,
        int & axis2) {
    const double parallelDot = 1.0 - epsilon;
    for (int i = 0; i < 9; ++i) {
        if (ca[i] >= parallelDot) {
            axis1 = i / 3;
            axis2 = i % 3;
            return true;
        }
    }
    return false;
}




void CollisionDetection::fillSolidBoxSolidBoxInfo(
        const Box & box1,
        const Box & box2,
        Vector3 & a,
        Vector3 & b,
        Vector3 & D,
        double* c,
        double* ca,
        double* ad,
        double* bd) {
    // length between center and each side of box1 and box2
    a = box1.extent() * 0.5;
    b = box2.extent() * 0.5;

    // difference between centers of box1 and box2
    D = box2.center() - box1.center();

    // store the value of all possible dot products between the
    // axes of box1 and box2, c_{row, col} in the Eberly paper
    // corresponds to c[row * 3 + col] for this 9 element array.
    //
    // c[] holds signed values, ca[] hold absolute values
    for (int i = 0; i < 9; ++i) {
        c[i] = dot(box1.axis(i / 3), box2.axis(i % 3));
        ca[i] = fabs(c[i]);
    }

    // store all possible dot products between the axes of box1 and D,
    // as well as the axes of box2 and D
    for (int i = 0; i < 3; ++i) {
        ad[i] = dot(box1.axis(i), D);
        bd[i] = dot(box2.axis(i), D);
    }
}



bool CollisionDetection::conservativeBoxBoxTest(
        const Vector3 & a, const Vector3 & b, const Vector3 & D) {
    // do a quick bounding sphere test because it is relatively
    // cheap, (three dot products, two sqrts, and a few others)
    double boxRadius1 = a.magnitude();
    double boxRadius2 = b.magnitude();
    return (D.squaredMagnitude() < square(boxRadius1 + boxRadius2));
}




bool CollisionDetection::fixedSolidBoxIntersectsFixedSolidBox(
    const Box&      box1,
    const Box&      box2,
    const int        lastSeparatingAxis) {
    // for explanations of the variable please refer to the
    // paper and fillSolidBoxSolidBoxInfo()
    Vector3 a;
    Vector3 b;
    Vector3 D;
    double c[9];
    double ca[9];
    double ad[3];
    double bd[3];

    fillSolidBoxSolidBoxInfo(box1, box2, a, b, D, c, ca, ad, bd);

    int dummy1, dummy2;
    bool parallelAxes = parallelAxisForSolidBoxSolidBox(ca, 0.00001,
            dummy1, dummy2);

    // check the separating axis from the last time step
    if (lastSeparatingAxis != -1 &&
            (lastSeparatingAxis < 6 || !parallelAxes)) {
        double projectedDistance = projectedDistanceForSolidBoxSolidBox(
                lastSeparatingAxis, a, b, D, c, ca, ad, bd);

        // the separating axis from the last time step is still
        // valid, the boxes do not intersect
        if (projectedDistance > 0.0) {
            return false;
        }
    }

    // test if the boxes can be separated by a plane normal to
    // any of the three axes of box1, any of the three axes of box2,
    // or any of the 9 possible cross products of axes from box1
    // and box2
    for (int i = 0; i < 15; ++i) {
        // do not need to check edge-edge cases if any two of
        // the axes are parallel
        if (parallelAxes && i == 6) {
            return true;
        }

        double projectedDistance =
            projectedDistanceForSolidBoxSolidBox(i, a, b, D, c, ca, ad, bd);

        // found a separating axis, the boxes do not intersect
        if (projectedDistance > 0.0) {
            return false;
        }
    }

    return true;
}



void CollisionDetection::closestPointsBetweenLineAndLine(
        const Line & line1,
        const Line & line2,
        Vector3 & closest1,
        Vector3 & closest2) {
    // TODO make accessors for Line that don't make a copy of data
    Vector3 P0 = line1.point();
    Vector3 u = line1.direction();
    Vector3 Q0 = line2.point();
    Vector3 v = line2.direction();
    Vector3 w0 = P0 - Q0;

    // a = 1.0, c = 1.0
    double  b = dot(u, v);
    double  d = dot(u, w0);
    double  e = dot(v, w0);
    double  D = 1.0 - b * b;
    double  sc, tc;

    static const double epsilon = 0.00001;

    if (D < epsilon) {
        // lines are parallel, choose P0 as one point, find the point
        // on line2 that is closest to P0
        sc = 0.0;
        tc = (b > 1.0) ? (d / b) : (e / 1.0);
    } else {
        // lines are not parallel
        sc = (b * e - 1.0 * d) / D;
        tc = (1.0 * e - b * d) / D;
    }

    closest1 = P0 + (sc * u);
    closest2 = Q0 + (tc * v);
}



float CollisionDetection::penetrationDepthForFixedBoxFixedBox(
    const Box&      box1,
    const Box&      box2,
    Array<Vector3>& contactPoints,
    Array<Vector3>& contactNormals,
    const int lastSeparatingAxis) {

    contactPoints.resize(0, DONT_SHRINK_UNDERLYING_ARRAY);
    contactNormals.resize(0, DONT_SHRINK_UNDERLYING_ARRAY);

    Vector3 a;
    Vector3 b;
    Vector3 D;
    double c[9];
    double ca[9];
    double ad[3];
    double bd[3];

    debugAssert(lastSeparatingAxis >= -1);
    debugAssert(lastSeparatingAxis < 15);

    fillSolidBoxSolidBoxInfo(box1, box2, a, b, D, c, ca, ad, bd);

    int axis1, axis2;
    bool parallelAxes = parallelAxisForSolidBoxSolidBox(ca, 0.00001,
            axis1, axis2);


    // check the separating axis from the last time step
    if (lastSeparatingAxis != -1 &&
            (lastSeparatingAxis < 6 || !parallelAxes)) {
        float projectedDistance = projectedDistanceForSolidBoxSolidBox(
                lastSeparatingAxis, a, b, D, c, ca, ad, bd);

        // the separating axis from the last time step is still
        // valid, the boxes do not intersect
        if (projectedDistance > 0.0) {
            return -projectedDistance;
        }
    }

    // test if the boxes can be separated by a plane normal to
    // any of the three axes of box1, any of the three axes of box2,
    // (test 9 possible cross products later)
    float penetration = -finf();
    int penetrationAxisIndex = -1;

    for (int i = 0; i < 6; ++i) {
        float projectedDistance =
            projectedDistanceForSolidBoxSolidBox(i, a, b, D, c, ca, ad, bd);

        // found a separating axis, the boxes do not intersect
        if (projectedDistance > 0.0) {
            return -projectedDistance;
        }

        // keep track of the axis that is least violated
        if (projectedDistance > penetration) {
            penetration = projectedDistance;
            penetrationAxisIndex = i;
        }
    }


    // for each edge-edge case we have to adjust the magnitude of
    // penetration since we did not include the dot(L, L) denominator
    // that can be smaller than 1.0 for the edge-edge cases.

    if (!parallelAxes) {
        double edgeDistances[9];

        // run through edge-edge cases to see if we can find a separating axis
        for (int i = 6; i < 15; ++i) {
            float projectedDistance =
                projectedDistanceForSolidBoxSolidBox(i, a, b, D, c, ca, ad, bd);

            // found a separating axis, the boxes do not intersect,
            // correct magnitude and return projected distance
            if (projectedDistance > 0.0) {
                Vector3 L = separatingAxisForSolidBoxSolidBox(i, box1, box2);
                projectedDistance /= dot(L, L);
                return -projectedDistance;
            }

            edgeDistances[i - 6] = projectedDistance;
        }

        // no separating axis found, the boxes do intersect,
        // correct the magnitudes of the projectedDistance values
        for (int i = 6; i < 15; ++i) {
            // find the negative penetration value with the smallest magnitude,
            // the adjustment done for the edge-edge cases only increases
            // magnitude by dividing by a number smaller than 1 and greater than 0
            float projectedDistance = (float)edgeDistances[i - 6];
            if (projectedDistance > penetration) {
                Vector3 L = separatingAxisForSolidBoxSolidBox(i, box1, box2);
                projectedDistance /= dot(L, L);
                if (projectedDistance > penetration) {
                    penetration = projectedDistance;
                    penetrationAxisIndex = i;
                }
            }
        }
    }

    // get final separating axis vector
    Vector3 L = separatingAxisForSolidBoxSolidBox(penetrationAxisIndex,
            box1, box2);

    // set L to be the normal that faces away from box1
    if (dot(L, D) < 0) {
        L = -L;
    }

    Vector3 contactPoint;

    if (penetrationAxisIndex < 6) {
        // vertex to face collision, find deepest colliding vertex
        const Box* vertexBox;
        Vector3 faceNormal = L;

        // L will be the outward facing normal for the faceBox
        if (penetrationAxisIndex < 3) {
            vertexBox = & box2;
            if (dot(L, D) < 0) {
                faceNormal = -L;
            }
        } else {
            vertexBox = & box1;
            if (dot(L, D) > 0) {
                faceNormal = -L;
            }
        }

        // find the vertex that is farthest away in the direction
        // face normal direction
        int deepestPointIndex = 0;
        float deepestPointDot = dot(faceNormal, vertexBox->corner(0));
        for (int i = 1; i < 8; ++i) {
            float dotProduct = dot(faceNormal, vertexBox->corner(i));
            if (dotProduct < deepestPointDot) {
                deepestPointDot = dotProduct;
                deepestPointIndex = i;
            }
        }
        
        // return the point half way between the deepest point and the
        // contacting face
        contactPoint = vertexBox->corner(deepestPointIndex) +
            (-penetration * 0.5 * faceNormal);
    } else {
        // edge-edge case, find the two ege lines
        int edge1 = (penetrationAxisIndex - 6) / 3;
        int edge2 = (penetrationAxisIndex - 6) % 3;
        Vector3 linePoint1 = box1.center();
        Vector3 linePoint2 = box2.center();
        Vector3 lineDir1;
        Vector3 lineDir2;

        // find edge line by finding the edge axis, and the
        // other two axes that are closest to the other box
        for (int i = 0; i < 3; ++i) {
            if (i == edge1) {
                lineDir1 = box1.axis(i);
            } else {
                Vector3 axis = box1.axis(i);
                if (dot(axis, L) < 0) {
                    axis = -axis;
                }
                linePoint1 += axis * a[i];
            }

            if (i == edge2) {
                lineDir2 = box2.axis(i);
            } else {
                Vector3 axis = box2.axis(i);
                if (dot(axis, L) > 0) {
                    axis = -axis;
                }
                linePoint2 += axis * b[i];
            }
        }

        // make lines from the two closest edges, and find
        // the points that on each line that are closest to the other
        Line line1 = Line::fromPointAndDirection(linePoint1, lineDir1);
        Line line2 = Line::fromPointAndDirection(linePoint2, lineDir2);
        Vector3 closest1;
        Vector3 closest2;

        closestPointsBetweenLineAndLine(line1, line2, closest1, closest2);
        
        // take the average of the two closest edge points for the final
        // contact point
        contactPoint = (closest1 + closest2) * 0.5;
    }

    contactPoints.push(contactPoint);
    contactNormals.push(L);

    return -penetration;

}




float CollisionDetection::penetrationDepthForFixedSphereFixedBox(
    const Sphere&   sphere,
    const Box&      box,
    Array<Vector3>& contactPoints,
    Array<Vector3>& contactNormals) {

    contactPoints.resize(0, DONT_SHRINK_UNDERLYING_ARRAY);
    contactNormals.resize(0, DONT_SHRINK_UNDERLYING_ARRAY);

    // In its local coordinate frame, the box measures
    // 2 * halfExtent[a] along dimesion a.
    Vector3 halfExtent(box.extent(0), box.extent(1), box.extent(2));
    halfExtent *= 0.5f;

    CoordinateFrame boxFrame;
    box.getLocalFrame(boxFrame);

    // Transform the sphere to the box's coordinate frame.
    Vector3 center = boxFrame.pointToObjectSpace(sphere.center);

    // Find the square of the distance from the sphere to the box


    // Distance along each axis from the closest side of the box
    // to the sphere center.  Negative values are *inside* the box.
    Vector3 distOutsideBox;

    // Divide space up into the 27 regions corresponding
    // to {+|-|0}X, {+|-|0}Y, {+|-|0}Z and classify the
    // sphere center into one of them.
    Vector3 centerRegion;

    // In the edge collision case, the edge is between vertices
    // (constant + variable) and (constant - variable).
    Vector3 constant, variable;

    int numNonZero = 0;

    // Iterate over axes
    for (int a = 0; a < 3; ++a) { 
        // For each (box side), see which direction the sphere
        // is outside the box (positive or negative).  Add the
        // square of that distance to the total distance from 
        // the box.

        float distanceFromLow  = -halfExtent[a] - center[a];
        float distanceFromHigh = center[a] - halfExtent[a];

        if (fabsf(distanceFromLow) < fabsf(distanceFromHigh)) {
            distOutsideBox[a] = distanceFromLow;
        } else {
            distOutsideBox[a] = distanceFromHigh;
        }

        if (distanceFromLow < 0.0) {
            if (distanceFromHigh < 0.0) {
                // Inside the box
                centerRegion[a] = 0.0;
                variable[a]     = 1.0;
            } else {
                // Off the high side
                centerRegion[a] = 1.0;
                constant[a]     = halfExtent[a];
                ++numNonZero;
            }
        } else if (distanceFromHigh < 0.0) {
            // Off the low side
            centerRegion[a] = -1.0;
            constant[a]     = -halfExtent[a];
            ++numNonZero;
        } else {
            debugAssertM(false, 
                "distanceFromLow and distanceFromHigh cannot both be positive");
        }
    }

    // Squared distance between the outside of the box and the
    // sphere center.
    float d2 = Vector3::zero().max(distOutsideBox).squaredMagnitude();

    if (d2 > square(sphere.radius)) {
        // There is no penetration because the distance is greater
        // than the radius of the sphere.  This is the common case
        // and we quickly exit.
        return -1;
    }

    // We know there is some penetration but need to classify it.
    //
    // Examine the region that contains the center of the sphere. If
    // there is exactly one non-zero axis, the collision is with a 
    // plane.  If there are exactly two non-zero axes, the collision
    // is with an edge.  If all three axes are non-zero, the collision is
    // with a vertex.  If there are no non-zero axes, the center is inside
    // the box.

    double depth = -1;
    switch (numNonZero) {
    case 3: // Vertex collision
        // The collision point is the vertex at constant, the normal
        // is the vector from there to the sphere center.
        contactNormals.append(boxFrame.normalToWorldSpace(constant - center));
        contactPoints.append(boxFrame.pointToWorldSpace(constant));
        depth = sphere.radius - sqrt(d2);
        break;

    case 2: // Edge collision
        {
            // TODO: unwrapping the edge constructor and closest point
            // code will probably make it faster.

            // Determine the edge
            Line line = Line::fromPointAndDirection(constant, variable);

            // Penetration depth:
            depth = sphere.radius - sqrt(d2);

            // The contact point is the closes point to the sphere on the line 
            Vector3 X = line.closestPoint(center);
            contactNormals.append(boxFrame.normalToWorldSpace(X - center).direction());
            contactPoints.append(boxFrame.pointToWorldSpace(X));
        }
        break;

    case 1: // Plane collision
        {
            // The plane normal is the centerRegion vector,
            // so the sphere normal is the negative.  Take
            // it to world space from box-space.

            // Center region doesn't need to be normalized because
            // it is known to contain only one non-zero value
            // and that value is +/- 1.
            Vector3 N = boxFrame.normalToWorldSpace(-centerRegion);
            contactNormals.append(N);

            // Penetration depth:
            depth = sphere.radius - sqrtf(d2);

            // Compute the contact point from the penetration depth
            contactPoints.append(sphere.center + N * (sphere.radius - depth));
        }
        break;

    case 0: // Volume collision

        // The sphere center is inside the box.  This is an easy case
        // to handle.  Note that all axes of distOutsideBox must
        // be negative.  
    
        // Arbitratily choose the sphere center as a contact point
        contactPoints.append(sphere.center);

        // Find the least-negative penetration axis.
        //
        // We could have computed this during the loop over the axes,
        // but since volume collisions are rare (they only occur with
        // large time steps), this case will seldom be executed and
        // should not be optimized at the expense of the others.
        if (distOutsideBox.x > distOutsideBox.y) {
            if (distOutsideBox.x > distOutsideBox.z) {
                // Smallest penetration on x-axis
                // Chose normal based on which side we're closest to.
                // Keep in mind that this is a normal to the sphere,
                // so it is the inverse of the box normal.
                if (center.x > 0) {
                    contactNormals.append(boxFrame.normalToWorldSpace(-Vector3::unitX()));
                } else {
                    contactNormals.append(boxFrame.normalToWorldSpace(Vector3::unitX()));
                }
                depth = -distOutsideBox.x;
            } else {
                // Smallest penetration on z-axis
                goto ZAXIS;
            }
        } else if (distOutsideBox.y > distOutsideBox.z) {
            // Smallest penetration on y-axis
            // Chose normal based on which side we're closest to.
            // Keep in mind that this is a normal to the sphere,
            // so it is the inverse of the box normal.
            if (center.y > 0) {
                contactNormals.append(boxFrame.normalToWorldSpace(-Vector3::unitY()));
            } else {
                contactNormals.append(boxFrame.normalToWorldSpace(Vector3::unitY()));
            }
            depth = -distOutsideBox.y;
        } else {
            // Smallest on z-axis
ZAXIS:
            // Chose normal based on which side we're closest to.
            // Keep in mind that this is a normal to the sphere,
            // so it is the inverse of the box normal.
            if (center.z > 0) {
                contactNormals.append(boxFrame.normalToWorldSpace(-Vector3::unitZ()));
            } else {
                contactNormals.append(boxFrame.normalToWorldSpace(Vector3::unitZ()));
            }
            depth = -distOutsideBox.z;
        }
        break;

    default:
        debugAssertM(false, "Fell through switch");
        break;
    }

    return depth;
}


float CollisionDetection::penetrationDepthForFixedSphereFixedSphere(
    const Sphere&           sphereA,
    const Sphere&           sphereB,
    Array<Vector3>&         contactPoints,
    Array<Vector3>&         contactNormals) {

    Vector3 axis = sphereB.center - sphereA.center;
    double radius = sphereA.radius + sphereB.radius;
    double mag = axis.magnitude();
    axis /= mag;
    double depth = -(mag - radius);

    contactPoints.resize(0, DONT_SHRINK_UNDERLYING_ARRAY);
    contactNormals.resize(0, DONT_SHRINK_UNDERLYING_ARRAY);

    if (depth >= 0) {
        contactPoints.append(sphereA.center + axis * (sphereA.radius - depth / 2));
        contactNormals.append(axis);
    }

    return depth;
}


float CollisionDetection::penetrationDepthForFixedSphereFixedPlane(
    const Sphere&           sphereA,
    const Plane&            planeB,
    Array<Vector3>&         contactPoints,
    Array<Vector3>&         contactNormals) {

    Vector3 N;
    double d;

    planeB.getEquation(N, d);
    
    double depth = -(sphereA.center.dot(N) + d - sphereA.radius);

    contactPoints.resize(0, DONT_SHRINK_UNDERLYING_ARRAY);
    contactNormals.resize(0, DONT_SHRINK_UNDERLYING_ARRAY);

    if (depth >= 0) {
        contactPoints.append(N * (depth - sphereA.radius) + sphereA.center);
        contactNormals.append(N);
    }

    return depth;
}


float CollisionDetection::penetrationDepthForFixedBoxFixedPlane(
    const Box&          box,
    const Plane&        plane,
    Array<Vector3>&     contactPoints,
    Array<Vector3>&     contactNormals) {

    Vector3 N;
    double d;
    
    plane.getEquation(N, d);

    contactPoints.resize(0, DONT_SHRINK_UNDERLYING_ARRAY);
    contactNormals.resize(0, DONT_SHRINK_UNDERLYING_ARRAY);

    float lowest = finf();
    for (int i = 0; i < 8; ++i) {
        const Vector3 vertex = box.corner(i);
        
        float x = vertex.dot(N) + (float)d;
        
        if (x <= 0) {
            // All vertices below the plane should be contact points.
            contactPoints.append(vertex);
            contactNormals.append(-N);
        }

        lowest = min(lowest, x);
    }

    // Depth should be a positive number
    return -lowest;
}


float CollisionDetection::collisionTimeForMovingPointFixedPlane(
    const Vector3&  point,
    const Vector3&  velocity,
    const Plane&    plane,
    Vector3&        location,
    Vector3&        outNormal) {

    // Solve for the time at which normal.dot(point + velocity) + d == 0.
    float d;
    Vector3 normal;
    plane.getEquation(normal, d);
    
    const float vdotN = velocity.dot(normal);
    const float pdotN = point.dot(normal);

    if (fuzzyEq(pdotN + d, 0.0f)) {
        // The point is *in* the plane.
        location = point;
        outNormal = normal;
        return 0;
    }

    if (vdotN >= 0.0f) {
        // no collision will occur
        location = Vector3::inf();
        return finf();
    }

    const float t = -(pdotN + d) / vdotN;
    if (t < 0) {
        location = Vector3::inf();
        return finf();
    } else {
        location = point + velocity * t;
        outNormal = normal;
        return t;
    }
}

bool __fastcall CollisionDetection::rayAABox(
    const Ray&              ray,
    const Vector3&          invDir,
    const AABox&            box,
    const Vector3&          boxCenter,
    float                   boundingRadiusSquared,
    Vector3&                location,
    bool&                   inside) {

    debugAssertM(fabs(ray.direction().squaredLength() - 1.0f) < 0.01f, format("Length = %f", ray.direction().length()));
    {
        // Pre-emptive partial bounding sphere test
        const Vector3 L(boxCenter - ray.origin());
        float d = L.dot(ray.direction());

        float L2 = L.dot(L);
        float D2 = square(d);
        float M2 = L2 - D2;

        if (((d < 0) && (L2 > boundingRadiusSquared)) || (M2 > boundingRadiusSquared)) {
            inside = false;
            return false;
        }
        // Passing here does not mean that the ray hits the bounding sphere;
        // we would still have to perform more expensive tests to determine
        // that.
    }

    inside = true;
    const Vector3& MinB = box.low();
    const Vector3& MaxB = box.high();
    Vector3 MaxT(-1.0f, -1.0f, -1.0f);

    // Find candidate planes.
    for (int i = 0; i < 3; ++i) {
        if (ray.origin()[i] < MinB[i]) {
            location[i]    = MinB[i];
            inside      = false;
            
            // Calculate T distances to candidate planes
            if (ray.direction()[i] != 0) {
                MaxT[i] = (MinB[i] - ray.origin()[i]) * invDir[i];
            }
        } else if (ray.origin()[i] > MaxB[i]) {
            location[i]    = MaxB[i];
            inside      = false;

            // Calculate T distances to candidate planes
            if (ray.direction()[i] != 0) {
                MaxT[i] = (MaxB[i] - ray.origin()[i]) * invDir[i];
            }
        }
    }

    if (inside) {
        // Ray origin inside bounding box
        location = ray.origin();
        return true;
    }
    
    // Get largest of the maxT's for final choice of intersection
    int WhichPlane = 0;
    if (MaxT[1] > MaxT[WhichPlane]) {
        WhichPlane = 1;
    }

    if (MaxT[2] > MaxT[WhichPlane]) {
        WhichPlane = 2;
    }

    // Check final candidate actually inside box
    if (MaxT[WhichPlane] < 0.0f) {
        // Miss the box
        return false;
    }

    for (int i = 0; i < 3; ++i) {
        if (i != WhichPlane) {
            location[i] = ray.origin()[i] + MaxT[WhichPlane] * ray.direction()[i];
            if ((location[i] < MinB[i]) ||
                (location[i] > MaxB[i])) {
                // On this plane we're outside the box extents, so
                // we miss the box
                return false;
            }
        }
    }
    
    return true;
}

float CollisionDetection::collisionTimeForMovingPointFixedSphere(
    const Vector3&  point,
    const Vector3&  velocity,
    const Sphere&   sphere,
    Vector3&        location,
    Vector3&        outNormal,
    bool            solid) {

    if (solid && sphere.contains(point)) {
        location = point;
        outNormal = (point - sphere.center).direction();
        return 0.0f;
    }

    float          speed = velocity.magnitude();
    const Vector3& direction = velocity / speed;

    // length of the axis between the start and the sphere
    const Vector3& L = sphere.center - point;
    float          d = L.dot(direction);

    float L2 = L.dot(L);
    float R2 = square(sphere.radius);
    float D2 = square(d);

    if ((d < 0.0f) && (L2 > R2)) {
        location = Vector3::inf();
        return finf();
    }

    const float M2 = L2 - D2;

    if (M2 > R2) {
        location = Vector3::inf();
        return finf();
    }

    float q = sqrt(R2 - M2);
    float time;

    if (L2 > R2) {
        time = d - q;
    } else {
        time = d + q;
    }

    time /= speed;

    location  = point + velocity * time;
    outNormal = (location - sphere.center).direction();

    return time;
}


float CollisionDetection::collisionTimeForMovingSphereFixedSphere(
    const Sphere&   movingSphere,
    const Vector3&  velocity,
    const Sphere&   fixedSphere,
    Vector3&        location,
    Vector3&        outNormal) {

    const Vector3& sep = (fixedSphere.center - movingSphere.center);
    float sepLen = sep.squaredLength();
    if (sepLen < square(movingSphere.radius + fixedSphere.radius)) {
        // Interpenetrating
        outNormal   = sep.directionOrZero();
        location = fixedSphere.center - outNormal * fixedSphere.radius;
        return 0;
    }

    float time = collisionTimeForMovingPointFixedSphere
        (movingSphere.center, velocity, 
         Sphere(fixedSphere.center, fixedSphere.radius + movingSphere.radius), 
         location, outNormal);

    if (time < finf()) {
        // Location is now the center of the moving sphere at the collision time.
        // Adjust for the size of the moving sphere.  Two spheres always collide
        // along a line between their centers.
        location += (location - fixedSphere.center) * movingSphere.radius / fixedSphere.radius;
    }

    return time;
}


/*
float CollisionDetection::collisionTimeForMovingPointFixedTriangle(
    const Vector3&            point,
    const Vector3&            velocity,
    const Triangle&       triangle,
    Vector3&                outLocation,
    Vector3&                outNormal) {

    double time = collisionTimeForMovingPointFixedPlane(point, velocity, triangle.plane(), outLocation, outNormal);

    if (time == finf()) {
        // No collision with the plane of the triangle.
        return finf();
    }

    if (isPointInsideTriangle(triangle.vertex(0), triangle.vertex(1), triangle.vertex(2), triangle.normal(), outLocation, triangle.primaryAxis())) {
        // Collision occured inside the triangle
        return time;
    } else {
        // Missed the triangle
        outLocation = Vector3::inf();
        return finf();
    }
}*/

/*
float CollisionDetection::collisionTimeForMovingPointFixedTriangle(
    const Vector3& orig,
    const Vector3& dir,
    const Vector3& vert0,
    const Vector3& vert1,
    const Vector3& vert2) {

    // Barycenteric coords
    double u, v;
    #define EPSILON 0.000001
    #define CROSS(dest,v1,v2) \
              dest[0]=v1[1]*v2[2]-v1[2]*v2[1]; \
              dest[1]=v1[2]*v2[0]-v1[0]*v2[2]; \
              dest[2]=v1[0]*v2[1]-v1[1]*v2[0];

    #define DOT(v1,v2) (v1[0]*v2[0]+v1[1]*v2[1]+v1[2]*v2[2])

    #define SUB(dest,v1,v2) \
              dest[0]=v1[0]-v2[0]; \
              dest[1]=v1[1]-v2[1]; \
              dest[2]=v1[2]-v2[2]; 

    double edge1[3], edge2[3], tvec[3], pvec[3], qvec[3];
    
    // find vectors for two edges sharing vert0
    SUB(edge1, vert1, vert0);
    SUB(edge2, vert2, vert0);
    
    // begin calculating determinant - also used to calculate U parameter
    CROSS(pvec, dir, edge2);
    
    // if determinant is near zero, ray lies in plane of triangle
    const double det = DOT(edge1, pvec);
    
    if (det < EPSILON) {
        return finf();
    }
    
    // calculate distance from vert0 to ray origin
    SUB(tvec, orig, vert0);
    
    // calculate U parameter and test bounds
    u = DOT(tvec, pvec);
    if ((u < 0.0) || (u > det)) {
        // Hit the plane outside the triangle
        return finf();
    }
    
    // prepare to test V parameter
    CROSS(qvec, tvec, edge1);
    
    // calculate V parameter and test bounds
    v = DOT(dir, qvec);
    if ((v < 0.0) || (u + v > det)) {
        // Hit the plane outside the triangle
        return finf();
    }
    
    // calculate t, scale parameters, ray intersects triangle
    // If we want u,v, we can compute this
    // double t = DOT(edge2, qvec);
    //const double inv_det = 1.0 / det;
    //t *= inv_det;
    //u *= inv_det;
    //v *= inv_det;
    // return t;

    // Case where we don't need correct (u, v):

    const double t = DOT(edge2, qvec);
    
    if (t >= 0) {
        // Note that det must be positive
        return t / det;
    } else {
        // We had to travel backwards in time to intersect
        return finf();
    }

    #undef EPSILON
    #undef CROSS
    #undef DOT
    #undef SUB
}
*/

float CollisionDetection::collisionTimeForMovingPointFixedBox(
    const Vector3&          point,
    const Vector3&          velocity,
    const Box&              box,
    Vector3&                location,
    Vector3&                outNormal) {

    double    bestTime;

    Vector3 normal;
    Vector3 v[4];

    // Prime the loop
    int f = 0;
    box.getFaceCorners(f, v[0], v[1], v[2], v[3]);
    bestTime = collisionTimeForMovingPointFixedRectangle(point, velocity, v[0], v[1], v[2], v[3], location, normal);
    outNormal = normal;

    // Check other faces
    for (f = 1; f < 6; ++f) {
        Vector3 pos;
        box.getFaceCorners(f, v[0], v[1], v[2], v[3]);
        float time = collisionTimeForMovingPointFixedRectangle(point, velocity, v[0], v[1], v[2], v[3], pos, normal);
        if (time < bestTime) {
            bestTime = time;
            outNormal = normal;
            location = pos;
        }
    }

    return bestTime;
}


float CollisionDetection::collisionTimeForMovingPointFixedAABox(
    const Vector3&          origin,
    const Vector3&          dir,
    const AABox&            box,
    Vector3&                location,
    bool&                   Inside,
    Vector3&                normal) {

    if (collisionLocationForMovingPointFixedAABox(origin, dir, box, location, Inside, normal)) {
        return (location - origin).magnitude();
    } else {
        return (float)finf();
    }
}


bool CollisionDetection::collisionLocationForMovingPointFixedAABox(
    const Vector3&          origin,
    const Vector3&          dir,
    const AABox&            box,
    Vector3&                location,
    bool&                   Inside,
    Vector3&                normal) {

    // Integer representation of a floating-point value.
    #define IR(x)    ((uint32&)x)

    Inside = true;
    const Vector3& MinB = box.low();
    const Vector3& MaxB = box.high();
    Vector3 MaxT(-1.0f, -1.0f, -1.0f);

    // Find candidate planes.
    for (int i = 0; i < 3; ++i) {
        if (origin[i] < MinB[i]) {
            location[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]) {
            location[i]    = MaxB[i];
            Inside        = false;

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

    if (Inside) {
        // Ray origin inside bounding box
        location = origin;
        return false;
    }

    // Get largest of the maxT's for final choice of intersection
    int 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) {
        // Miss the box
        return false;
    }

    for (int i = 0; i < 3; ++i) {
        if (i != WhichPlane) {
            location[i] = origin[i] + MaxT[WhichPlane] * dir[i];
            if ((location[i] < MinB[i]) ||
                (location[i] > MaxB[i])) {
                // On this plane we're outside the box extents, so
                // we miss the box
                return false;
            }
        }
    }

    // Choose the normal to be the plane normal facing into the ray
    normal = Vector3::zero();
    normal[WhichPlane] = (dir[WhichPlane] > 0) ? -1.0 : 1.0;

    return true;

    #undef IR
}



float CollisionDetection::collisionTimeForMovingPointFixedRectangle(
    const Vector3&      point,
    const Vector3&      velocity,
    const Vector3&      v0,
    const Vector3&      v1,
    const Vector3&      v2,
    const Vector3&      v3,
    Vector3&            location,
    Vector3&            outNormal) {

    Plane plane = Plane(v0, v1, v2);

    float time = collisionTimeForMovingPointFixedPlane(point, velocity, plane, location, outNormal);

    if (time == finf()) {
        // No collision is ever going to happen
        return time;
    }

    if (isPointInsideRectangle(v0, v1, v2, v3, plane.normal(), location)) {
        // The intersection point is inside the rectangle; that is the location where
        // the point hits the rectangle.
        return time;
    } else {
        return finf();
    }
}

/** Used by findRayCapsuleIntersection.
    @cite From magic software http://www.magic-software.com/Source/Intersection3D/MgcIntr3DLinCap.cpp */
static int findRayCapsuleIntersectionAux(
    const Vector3&        rkOrigin,
    const Vector3&        rkDirection,
    const Capsule&        rkCapsule,
    double               afT[2]) {

    Vector3 capsuleDirection = rkCapsule.point(1) - rkCapsule.point(0);

    // set up quadratic Q(t) = a*t^2 + 2*b*t + c
    Vector3 kW = capsuleDirection;
    float fWLength = kW.length();
    kW = kW.direction();

    Vector3 kU, kV;
    kW.getTangents(kU, kV);
    Vector3 kD(kU.dot(rkDirection), kV.dot(rkDirection), kW.dot(rkDirection));

    float fDLength = kD.length();
    kD = kD.direction();

    float fEpsilon = 1e-6f;

    float fInvDLength = 1.0f/fDLength;
    Vector3 kDiff = rkOrigin - rkCapsule.point(0);
    Vector3 kP(kU.dot(kDiff),kV.dot(kDiff),kW.dot(kDiff));
    float fRadiusSqr = square(rkCapsule.radius());

    float fInv, fA, fB, fC, fDiscr, fRoot, fT, fTmp;

    // Is the velocity parallel to the capsule direction? (or zero)
    if ((abs(kD.z) >= 1.0f - fEpsilon) || (fDLength < fEpsilon)) {

        float fAxisDir = rkDirection.dot(capsuleDirection);

        fDiscr = fRadiusSqr - kP.x*kP.x - kP.y*kP.y;
        if ((fAxisDir < 0) && (fDiscr >= 0.0f)) {
            // Velocity anti-parallel to the capsule direction
            fRoot = sqrt(fDiscr);
            afT[0] = (kP.z + fRoot)*fInvDLength;
            afT[1] = -(fWLength - kP.z + fRoot)*fInvDLength;
            return 2;
        } else if ((fAxisDir > 0) && (fDiscr >= 0.0f)) {
            // Velocity parallel to the capsule direction
            fRoot = sqrt(fDiscr);
            afT[0] = -(kP.z + fRoot)*fInvDLength;
            afT[1] = (fWLength - kP.z + fRoot)*fInvDLength;
            return 2;
        } else {
            // sphere heading wrong direction, or no velocity at all
            return 0;
        }   
    }

    // test intersection with infinite cylinder
    fA = kD.x*kD.x + kD.y*kD.y;
    fB = kP.x*kD.x + kP.y*kD.y;
    fC = kP.x*kP.x + kP.y*kP.y - fRadiusSqr;
    fDiscr = fB*fB - fA*fC;
    if (fDiscr < 0.0f) {
        // line does not intersect infinite cylinder
        return 0;
    }

    int iQuantity = 0;

    if (fDiscr > 0.0f) {
        // line intersects infinite cylinder in two places
        fRoot = sqrt(fDiscr);
        fInv = 1.0f/fA;
        fT = (-fB - fRoot)*fInv;
        fTmp = kP.z + fT*kD.z;
        if ((0.0f <= fTmp) && (fTmp <= fWLength)) {
            afT[iQuantity] = fT * fInvDLength;
            ++iQuantity;
        }

        fT = (-fB + fRoot)*fInv;
        fTmp = kP.z + fT*kD.z;
        
        if ((0.0f <= fTmp) && (fTmp <= fWLength)) {
            afT[iQuantity++] = fT*fInvDLength;
        }

        if (iQuantity == 2) {
            // line intersects capsule wall in two places
            return 2;
        }
    } else {
        // line is tangent to infinite cylinder
        fT = -fB/fA;
        fTmp = kP.z + fT*kD.z;
        if ((0.0f <= fTmp) && (fTmp <= fWLength)) {
            afT[0] = fT*fInvDLength;
            return 1;
        }
    }

    // test intersection with bottom hemisphere
    // fA = 1
    fB += kP.z*kD.z;
    fC += kP.z*kP.z;
    fDiscr = fB*fB - fC;
    if (fDiscr > 0.0f) {
        fRoot = sqrt(fDiscr);
        fT = -fB - fRoot;
        fTmp = kP.z + fT*kD.z;
        if (fTmp <= 0.0f) {
            afT[iQuantity++] = fT*fInvDLength;
            if (iQuantity == 2) {
                return 2;
            }
        }

        fT = -fB + fRoot;
        fTmp = kP.z + fT*kD.z;
        if (fTmp <= 0.0f) {
            afT[iQuantity++] = fT*fInvDLength;
            if (iQuantity == 2) {
                return 2;
            }
        }
    } else if (fDiscr == 0.0f) {
        fT = -fB;
        fTmp = kP.z + fT*kD.z;
        if (fTmp <= 0.0f) {
            afT[iQuantity++] = fT*fInvDLength;
            if (iQuantity == 2) {
                return 2;
            }
        }
    }

    // test intersection with top hemisphere
    // fA = 1
    fB -= kD.z*fWLength;
    fC += fWLength*(fWLength - 2.0f*kP.z);

    fDiscr = fB*fB - fC;
    if (fDiscr > 0.0f) {
        fRoot = sqrt(fDiscr);
        fT = -fB - fRoot;
        fTmp = kP.z + fT*kD.z;
        if (fTmp >= fWLength) {
            afT[iQuantity++] = fT*fInvDLength;
            if (iQuantity == 2) {
                return 2;
            }
        }

        fT = -fB + fRoot;
        fTmp = kP.z + fT*kD.z;
        if (fTmp >= fWLength) {
            afT[iQuantity++] = fT*fInvDLength;
            if (iQuantity == 2) {
                return 2;
            }
        }
    } else if (fDiscr == 0.0f) {
        fT = -fB;
        fTmp = kP.z + fT*kD.z;
        if (fTmp >= fWLength) {
            afT[iQuantity++] = fT*fInvDLength;
            if (iQuantity == 2) {
                return 2;
            }
        }
    }

    return iQuantity;
}


/** Used by collisionTimeForMovingPointFixedCapsule.
    @cite From magic software http://www.magic-software.com/Source/Intersection3D/MgcIntr3DLinCap.cpp
    
    @param rkRay      The ray
    @param rkCapsule  The capsule
    @param riQuantity The number of intersections found
    @param akPoint    The intersections found
    @return           True if there is at least one intersection
    */
static bool findRayCapsuleIntersection(
    const Ray&            rkRay,
    const Capsule&        rkCapsule,
    int&                riQuantity,
    Vector3                akPoint[2]) {

    double afT[2];
    riQuantity = findRayCapsuleIntersectionAux(rkRay.origin(), rkRay.direction(), rkCapsule, afT);

    // Only return intersections that occur in the future
    int iClipQuantity = 0;
    int i;
    for (i = 0; i < riQuantity; ++i) {
        if (afT[i] >= 0.0f) {
            akPoint[iClipQuantity] = rkRay.origin() + afT[i] * rkRay.direction();
            ++iClipQuantity;
        }
    }

    riQuantity = iClipQuantity;
    return (riQuantity > 0);
}

float CollisionDetection::collisionTimeForMovingPointFixedCapsule(
    const Vector3&        _point,
    const Vector3&        velocity,
    const Capsule&        capsule,
    Vector3&            location,
    Vector3&            outNormal) {

    float timeScale = velocity.magnitude();

    if (timeScale == 0.0f) {
        timeScale = 1;
    }

    Vector3 direction = velocity / timeScale;
    int numIntersections;
    Vector3 intersection[2];
    findRayCapsuleIntersection(Ray::fromOriginAndDirection(_point, direction), capsule, numIntersections, intersection);

    if (numIntersections == 2) {
        // A collision can only occur if there are two intersections.  If there is one
        // intersection, that one is exiting the capsule.  

        // Find the entering intersection (the first one that occurs).
        float d0 = (intersection[0] - _point).squaredMagnitude();
        float d1 = (intersection[1] - _point).squaredMagnitude();

        // Compute the surface normal (if we aren't ignoring the result)
        if (&outNormal != &ignore) {
            Vector3 p2 = LineSegment::fromTwoPoints(capsule.point(0), capsule.point(1)).closestPoint(_point);
            outNormal = (_point - p2).direction();
        }

        if (d0 > d1) {
            location = intersection[1];
            return sqrt(d1) / timeScale;
        } else {
            location = intersection[0];
            return sqrt(d0) / timeScale;
        }
    } else {
        // No entering intersection discovered; return no intersection.
        location = Vector3::inf();
        return finf();
    }
}


float CollisionDetection::collisionTimeForMovingSphereFixedPlane(
    const Sphere&        sphere,
    const Vector3&        velocity,
    const Plane&        plane,
    Vector3&            location,
    Vector3&            outNormal) {

    if (sphere.radius == 0) {
        // Optimization for zero radius sphere
        return collisionTimeForMovingPointFixedPlane(sphere.center, velocity, plane, location, outNormal);
    }

    // The world-space collision point, which lies on the surface of the sphere, will be the point at
    // center + velocity * time - (radius * planeNormal).  Collisions only occur when
    // the sphere is travelling into the plane.

    float d;
    plane.getEquation(outNormal, d);
    
    // Rate at which the sphere is approaching the plane
    const float vdotN = velocity.dot(outNormal);

    if (fuzzyGt(vdotN, 0)) {
        // No collision when the sphere is moving towards a backface.
        location = Vector3::inf();
        return (float)finf();
    }

    // Initial distance from the sphere center to the plane
    const float distance = sphere.center.dot(outNormal) + d;

    if (fuzzyLe(G3D::abs(distance), sphere.radius)) {
        // Already interpenetrating
        location = sphere.center - distance * outNormal;
        return 0;
    } else {
        // The point on the sphere (in world space) that will eventually first contact the plane
        const Point3& point = sphere.center - (sphere.radius * outNormal);

        // The problem is now reduced to finding when the point hits the plane
        return collisionTimeForMovingPointFixedPlane(point, velocity, plane, location, outNormal);
    }

}


float CollisionDetection::collisionTimeForMovingSphereFixedTriangle
(const Sphere&                 sphere,
 const Vector3&              velocity,
 const Triangle&             triangle,
 Vector3&                    outLocation,
 float                       b[3]) {

    if (velocity.dot(triangle.normal()) > 0.0f) {
        // No collision if moving towards a backface
        return finf();
    }
    Vector3 dummy;
    const float time = collisionTimeForMovingSphereFixedPlane(sphere, velocity, triangle.plane(), 
                                                        outLocation, dummy);

    if (time == finf()) {
        // No collision is ever going to happen
        return time;
    }

    // We will hit the plane of the triangle at *time*. See if
    // the intersection point actually is within the triangle.

    if (isPointInsideTriangle(triangle.vertex(0), triangle.vertex(1), triangle.vertex(2), triangle.normal(), 
        outLocation, b, triangle.primaryAxis())) {

        // The intersection point is inside the triangle; that is the location where
        // the sphere hits the triangle.

#       ifdef G3D_DEBUG
        {
            // Internal consistency checks
            debugAssertM(b[0] >= 0.0 && b[0] <= 1.0f, "Intersection is outside triangle.");
            debugAssertM(b[1] >= 0.0 && b[1] <= 1.0f, "Intersection is outside triangle.");
            debugAssertM(b[2] >= 0.0 && b[2] <= 1.0f, "Intersection is outside triangle.");
            const Vector3& blend = 
                b[0] * triangle.vertex(0) + 
                b[1] * triangle.vertex(1) + 
                b[2] * triangle.vertex(2);
            debugAssertM(blend.fuzzyEq(outLocation), "Barycentric coords don't match intersection.");
            // Call again so that we can debug the problem
            // isPointInsideTriangle(triangle.vertex(0), triangle.vertex(1), triangle.vertex(2), triangle.normal(), 
            // outLocation, b, triangle.primaryAxis());
        }
#       endif

        return time;
    }

    // The collision (if it exists) is with a point on the triangle perimeter.
    // Switch over to moving the triangle towards a fixed sphere and see at what time
    // they will hit.

    // Closest point on the triangle to the sphere intersection with the plane.
    int edgeIndex;
    const Vector3& point = closestPointOnTrianglePerimeter(triangle._vertex, triangle.edgeDirection,
                                                           triangle.edgeMagnitude, outLocation, edgeIndex);

    float t = 0;
    if (! sphere.contains(point)) {
        // The point is outside the sphere--see when it will hit
        t = collisionTimeForMovingPointFixedSphere(point, -velocity, sphere, dummy, dummy);
    }

    if (t < finf()) {
        outLocation = point;
        // Compute Barycentric coords

        // Index of the next vertex
        static const int next[] = {1, 2, 0};

        // Project along the edge in question.
        // Avoid sqrt by taking advantage of the existing edgeDirection unit vector.
        b[next[edgeIndex]] = (outLocation - triangle._vertex[edgeIndex]).dot
            (triangle.edgeDirection[edgeIndex]) / triangle.edgeMagnitude[edgeIndex];

        b[edgeIndex] = 1.0f - b[next[edgeIndex]];

        b[next[next[edgeIndex]]] = 0.0f;

#       ifdef G3D_DEBUG
        {
            // Internal consistency checks
            for (int i = 0; i < 3; ++i) {
                debugAssertM(fuzzyGe(b[i], 0.0f) && fuzzyLe(b[i], 1.0f), "Intersection is outside triangle.");
            }
            Vector3 blend = 
                b[0] * triangle.vertex(0) + 
                b[1] * triangle.vertex(1) + 
                b[2] * triangle.vertex(2);
            debugAssertM(blend.fuzzyEq(outLocation), 
                format("Barycentric coords don't match intersection. %s != %s", 
                    blend.toString().c_str(), 
                    outLocation.toString().c_str()));    

            // Call again so that we can debug the problem
            collisionTimeForMovingPointFixedSphere(point, -velocity, sphere, dummy, dummy);
        }
#       endif

        // Due to tiny roundoffs, these values might be slightly out of bounds.
        // Ensure that they are legal.  Note that the above debugging code
        // verifies that we are not clamping truly illegal values.
        for (int i = 0; i < 3; ++i) {
            b[i] = clamp(b[i], 0.0f, 1.0f);
        }
    }

    // The collision occured at the point, if it occured.  The normal
    // was the plane normal, computed above.

    return t;
}


float CollisionDetection::collisionTimeForMovingSphereFixedRectangle(
    const Sphere&       sphere,
    const Vector3&      velocity,
    const Vector3&      v0,
    const Vector3&      v1,
    const Vector3&      v2,
    const Vector3&      v3,
    Vector3&            location,
    Vector3&            outNormal) {

    Plane plane(v0, v1, v2);

    float time = collisionTimeForMovingSphereFixedPlane(sphere, velocity, plane, location, outNormal);

    if (time == finf()) {
        // No collision is ever going to happen
        return time;
    }

    if (isPointInsideRectangle(v0, v1, v2, v3, plane.normal(), location)) {
        // The intersection point is inside the rectangle; that is the location where
        // the sphere hits the rectangle.
        return time;
    }

    // Switch over to moving the rectangle towards a fixed sphere and see at what time
    // they will hit.

    Vector3 point = closestPointToRectanglePerimeter(v0, v1, v2, v3, sphere.center);

    Vector3 dummy;
    double t = collisionTimeForMovingPointFixedSphere(point, -velocity, sphere, location, dummy);

    // Normal is the plane normal, location is the original location of the point.
    location = point;

    return t;
}


float CollisionDetection::collisionTimeForMovingSphereFixedBox(
    const Sphere&       sphere,
    const Vector3&      velocity,
    const Box&          box,
    Vector3&            location,
    Vector3&            outNormal) {

    if (fixedSolidSphereIntersectsFixedSolidBox(sphere, box)) {
        // TODO: Compute more useful location and normal?
        location = sphere.center;
        outNormal = Vector3::zero();
        return 0;
    }

    float bestTime;

    Vector3 v[4];
    int f = 0;
    box.getFaceCorners(f, v[0], v[1], v[2], v[3]);
    bestTime = collisionTimeForMovingSphereFixedRectangle(sphere, velocity, v[0], v[1], v[2], v[3], location, outNormal);

    for (f = 1; f < 6; ++f) {
        Vector3 pos, normal;
        box.getFaceCorners(f, v[0], v[1], v[2], v[3]);
        float time = collisionTimeForMovingSphereFixedRectangle(sphere, velocity, v[0], v[1], v[2], v[3], pos, normal);
        if (time < bestTime) {
            bestTime  = time;
            location  = pos;
            outNormal = normal;
        }
    }

    return bestTime;
}


float CollisionDetection::collisionTimeForMovingSphereFixedCapsule(
    const Sphere&        sphere,
    const Vector3&        velocity,
    const Capsule&        capsule,
    Vector3&            location,
    Vector3&            outNormal) {

    (void)outNormal;

    Capsule _capsule(capsule.point(0), capsule.point(1), capsule.radius() + sphere.radius);

    Vector3 normal;
    double time = collisionTimeForMovingPointFixedCapsule(sphere.center, velocity, _capsule, location, normal);
    
    if (time < finf()) {
        // Location is now the position of the center of the sphere at the time of collision.
        // We have to adjust the collision location for the size of the sphere.
        location -= sphere.radius * normal;
    }

    return time;
}


Vector3 CollisionDetection::bounceDirection(
    const Sphere&   sphere,
    const Vector3&  velocity,
    const float     collisionTime,
    const Vector3&  collisionLocation,
    const Vector3&  collisionNormal) {

    // Location when the collision occurs
    Vector3 sphereLocation  = sphere.center + velocity * collisionTime;

    Vector3 normal          = (sphereLocation - collisionLocation);
    if (fuzzyEq(normal.squaredMagnitude(), 0)) {
        normal = collisionNormal;
    } else {
        normal = normal.direction();
    }

    Vector3 direction       = velocity.direction();

    // Reflect direction about the normal
    return direction - 2.0 * normal * normal.dot(direction);
}


Vector3 CollisionDetection::slideDirection(
    const Sphere&   sphere,
    const Vector3&  velocity,
    const float     collisionTime,
    const Vector3&  collisionLocation) {

    Vector3 sphereLocation  = sphere.center + velocity * collisionTime;
    Vector3 normal          = (sphereLocation - collisionLocation).direction();
    Vector3 direction       = velocity.direction();

    // subtract off the part in the direction away from the normal.
    return direction - normal * normal.dot(direction);
}


Vector3 CollisionDetection::closestPointOnLineSegment(
    const Vector3& v0,
    const Vector3& v1,
    const Vector3& point) {

    const Vector3& edge       = (v1 - v0);
    float          edgeLength = edge.magnitude();

    if (edgeLength == 0) {
        // The line segment is a point
        return v0;
    }

    return closestPointOnLineSegment(v0, v1, edge / edgeLength, edgeLength, point);
}


Vector3 CollisionDetection::closestPointOnLineSegment(
    const Vector3& v0,
    const Vector3& v1,
    const Vector3& edgeDirection,
    const float    edgeLength,
    const Vector3& point) {

    debugAssert((v1 - v0).direction().fuzzyEq(edgeDirection));
    debugAssert(fuzzyEq((v1 - v0).magnitude(), edgeLength));

    // Vector towards the point
    const Vector3& c = point - v0;

    // Projected onto the edge itself
    float t = edgeDirection.dot(c);

    if (t <= 0) {
        // Before the start
        return v0;
    } else if (t >= edgeLength) {
        // After the end
        return v1;
    } else {
        // At distance t along the edge
        return v0 + edgeDirection * t;
    }
}


Vector3 CollisionDetection::closestPointOnTrianglePerimeter(
    const Vector3&            v0, 
    const Vector3&            v1,
    const Vector3&            v2,
    const Vector3&            point) {
    
    Vector3 v[3] = {v0, v1, v2};
    Vector3 edgeDirection[3] = {(v1 - v0), (v2 - v1), (v0 - v2)};
    float   edgeLength[3];
    
    for (int i = 0; i < 3; ++i) {
        edgeLength[i] = edgeDirection[i].magnitude();
        edgeDirection[i] /= edgeLength[i];
    }

    int edgeIndex;
    return closestPointOnTrianglePerimeter(v, edgeDirection, edgeLength, point, edgeIndex);
}


Vector3 CollisionDetection::closestPointOnTrianglePerimeter(
    const Vector3   v[3],
    const Vector3   edgeDirection[3],
    const float     edgeLength[3],
    const Vector3&  point,
    int&            edgeIndex) {

    // Closest point on segment from v[i] to v[i + 1]
    Vector3 r[3];

    // Distance squared from r[i] to point
    float d[3];

    // Index of the next point
    static const int next[] = {1, 2, 0};

    for (int i = 0; i < 3; ++i) {
        r[i] = closestPointOnLineSegment(v[i], v[next[i]], edgeDirection[i], edgeLength[i], point);
        d[i] = (r[i] - point).squaredMagnitude();
    }

    if (d[0] < d[1]) {
        if (d[0] < d[2]) {
            // Between v0 and v1
            edgeIndex = 0;
        } else {
            // Between v2 and v0
            edgeIndex = 2;
        }
    } else {
        if (d[1] < d[2]) {
            // Between v1 and v2
            edgeIndex = 1;
        } else {
            // Between v2 and v0
            edgeIndex = 2;
        }
    }

#   ifdef G3D_DEBUG
    {
        Vector3 diff = r[edgeIndex] - v[edgeIndex];
        debugAssertM(fuzzyEq(diff.direction().dot(edgeDirection[edgeIndex]), 1.0f) ||
            diff.fuzzyEq(Vector3::zero()), "Point not on correct triangle edge");
        float frac = diff.dot(edgeDirection[edgeIndex])/edgeLength[edgeIndex];
        debugAssertM(frac >= -0.000001, "Point off low side of edge.");
        debugAssertM(frac <= 1.000001, "Point off high side of edge.");
    }
#   endif

    return r[edgeIndex];
}


bool CollisionDetection::isPointInsideTriangle(
    const Vector3&            v0,
    const Vector3&            v1,
    const Vector3&            v2,
    const Vector3&            normal,
    const Vector3&            point,
    float                   b[3],
    Vector3::Axis           primaryAxis) {
    
    if (primaryAxis == Vector3::DETECT_AXIS) {
        primaryAxis = normal.primaryAxis();
    }

    // Check that the point is within the triangle using a Barycentric
    // coordinate test on a two dimensional plane.
    int i, j;

    switch (primaryAxis) {
    case Vector3::X_AXIS:
        i = Vector3::Y_AXIS;
        j = Vector3::Z_AXIS;
        break;

    case Vector3::Y_AXIS:
        i = Vector3::Z_AXIS;
        j = Vector3::X_AXIS;
        break;

    case Vector3::Z_AXIS:
        i = Vector3::X_AXIS;
        j = Vector3::Y_AXIS;
        break;

    default:
        // This case is here to supress a warning on Linux
        i = j = 0;
        debugAssertM(false, "Should not get here.");
        break;
    }

    // See if all barycentric coordinates are non-negative

    // 2D area via cross product
#   define AREA2(d, e, f)  (((e)[i] - (d)[i]) * ((f)[j] - (d)[j]) - ((f)[i] - (d)[i]) * ((e)[j] - (d)[j]))

    // Area of the polygon
    float area = AREA2(v0, v1, v2);
    if (area == 0) {
        // This triangle has zero area, so the point must not
        // be in it unless the triangle point is the test point.
        return (v0 == point);
    }

    debugAssert(area != 0);

    float invArea = 1.0f / area;

    // (avoid normalization until absolutely necessary)
    b[0] = AREA2(point, v1, v2) * invArea;

    if ((b[0] < 0.0f) || (b[0] > 1.0f)) {
        return false;
    }

    b[1] = AREA2(v0,  point, v2) * invArea;
    if ((b[1] < 0.0f) || (b[1] > 1.0f)) {
        return false;
    }

    b[2] = 1.0f - b[0] - b[1];

#   undef AREA2

    return (b[2] >= 0.0f) && (b[2] <= 1.0f);
}


bool CollisionDetection::isPointInsideRectangle(
    const Vector3& v0,
    const Vector3& v1,
    const Vector3& v2,
    const Vector3& v3,
    const Vector3& normal,
    const Vector3& point) {

    return isPointInsideTriangle(v0, v1, v2, normal, point) ||
           isPointInsideTriangle(v2, v3, v0, normal, point);  
}


Vector3 CollisionDetection::closestPointToRectanglePerimeter(
    const Vector3& v0,
    const Vector3& v1,
    const Vector3& v2,
    const Vector3& v3,
    const Vector3& point) {

    Vector3 r0 = closestPointOnLineSegment(v0, v1, point);
    Vector3 r1 = closestPointOnLineSegment(v1, v2, point);
    Vector3 r2 = closestPointOnLineSegment(v2, v3, point);
    Vector3 r3 = closestPointOnLineSegment(v3, v0, point);

    double d0 = (r0 - point).squaredMagnitude();
    double d1 = (r1 - point).squaredMagnitude();
    double d2 = (r2 - point).squaredMagnitude();
    double d3 = (r3 - point).squaredMagnitude();

    if (d0 < d1) {
        if (d0 < d2) {
            if (d0 < d3) {
                return r0;
            } else {
                return r3;
            }
        } else {
            if (d2 < d3) {
                return r2;
            } else {
                return r3;
            }
        }
    } else {
        if (d1 < d2) {
            if (d1 < d3) {
                return r1;
            } else {
                return r3;
            }
        } else {
            if (d2 < d3) {
                return r2;
            } else {
                return r3;
            }
        }
    }
}


Vector3 CollisionDetection::closestPointToRectangle(
    const Vector3&      v0,
    const Vector3&      v1,
    const Vector3&      v2,
    const Vector3&      v3,
    const Vector3&      point) {

    Plane plane(v0, v1, v2);

    // Project the point into the plane
    double a, b, c, d;
    plane.getEquation(a, b, c, d);
    
    double distance = a*point.x + b*point.y + c*point.z + d;
    Vector3 planePoint = point - distance * plane.normal();

    if (isPointInsideRectangle(v0, v1, v2, v3, plane.normal(), planePoint)) {
        return planePoint;
    } else {
        return closestPointToRectanglePerimeter(v0, v1, v2, v3, planePoint);
    }
}


bool CollisionDetection::fixedSolidSphereIntersectsFixedSolidSphere(
    const Sphere&           sphere1,
    const Sphere&           sphere2) {
    
    return (sphere1.center - sphere2.center).squaredMagnitude() < square(sphere1.radius + sphere2.radius);
}


bool CollisionDetection::fixedSolidSphereIntersectsFixedSolidBox(
    const Sphere&           sphere,
    const Box&              box) {

    // If the center of the sphere is within the box, the whole
    // sphere is within the box.
    if (box.contains(sphere.center)) {
        return true;
    }

    float r2 = square(sphere.radius);

    // Find the closest point on the surface of the box to the sphere.  If
    // this point is within the sphere's radius, they intersect.
    int f;
    for (f = 0; f < 6; ++f) {
        Vector3 v0, v1, v2, v3;
        box.getFaceCorners(f, v0, v1, v2, v3);
        if ((closestPointToRectangle(v0, v1, v2, v3, sphere.center) - sphere.center).squaredMagnitude() <= r2) {
            return true;
        }
    }

    return false;
}


bool CollisionDetection::movingSpherePassesThroughFixedBox(
    const Sphere&           sphere,
    const Vector3&          velocity,
    const Box&              box,
    double                  timeLimit) {

    // If they intersect originally, they definitely pass through each other.
    if (fixedSolidSphereIntersectsFixedSolidBox(sphere, box)) {
        return true;
    }

    // See if the sphere hits the box during the time period.
    Vector3 dummy1, dummy2;

    return (collisionTimeForMovingSphereFixedBox(sphere, velocity, box, dummy1, dummy2) < timeLimit);
}


bool CollisionDetection::movingSpherePassesThroughFixedSphere(
    const Sphere&           sphere,
    const Vector3&          velocity,
    const Sphere&           fixedSphere,
    double                  timeLimit) {

    if (fixedSolidSphereIntersectsFixedSolidSphere(sphere, fixedSphere)) {
        return true;
    }

    // Extend the fixed sphere by the radius of the moving sphere
    Sphere bigFixed(fixedSphere.center, fixedSphere.radius + sphere.radius);
    Vector3 dummy1, dummy2;

    // If the sphere collides with the other sphere during the time limit, it passes through
    return (collisionTimeForMovingPointFixedSphere(sphere.center, velocity, bigFixed, dummy1, dummy2) < timeLimit);
}



bool CollisionDetection::fixedSolidSphereIntersectsFixedTriangle(
    const Sphere&           sphere,
    const Triangle&         triangle) {

    // How far is the sphere from the plane of the triangle
    const Plane& plane = triangle.plane();

    // Does the closest point to the sphere center lie within the triangle?
    Vector3 v = plane.closestPoint(sphere.center);

    // Is the closest point to the plane within the sphere?
    if ((v - sphere.center).squaredLength() <= square(sphere.radius)) {
        // Is it also within the triangle?
        float b[3];
        if (isPointInsideTriangle(triangle.vertex(0), triangle.vertex(1), triangle.vertex(2), triangle.normal(), 
                v, b, triangle.primaryAxis())){
            // The closest point is inside the triangle
            return true;
        }
    }

    // ignored
    int edgeIndex;

    v = closestPointOnTrianglePerimeter(triangle._vertex, triangle.edgeDirection, triangle.edgeMagnitude, sphere.center, edgeIndex);

    // Is the closest point within the sphere?
    return ((v - sphere.center).squaredLength() <= square(sphere.radius));
}


////////////////////////////////////////////////////////////////////////////////
// AABB-triangle overlap test code based on Tomas Akenine-Möller's
// http://www.cs.lth.se/home/Tomas_Akenine_Moller/code/tribox3.txt
// Ported 2008-12-28

#define X 0
#define Y 1
#define Z 2

#define FINDMINMAX(x0, x1, x2, min, max) \
    min = max = x0;   \
    if(x1<min) min=x1;\
    if(x1>max) max=x1;\
    if(x2<min) min=x2;\
    if(x2>max) max=x2;

static bool planeBoxOverlap(const Vector3& normal, const Vector3& vert, const Vector3& maxbox)  {
    Vector3 vmin, vmax;
    float v;
    
    // for each axis
    for(int a = 0; a < 3; ++a) {
        v = vert[a];
        
        if (normal[a] > 0.0f) {
            vmin[a] = -maxbox[a] - v;
            vmax[a] =  maxbox[a] - v;
        } else {
            vmin[a] =  maxbox[a] - v;
            vmax[a] = -maxbox[a] - v;
        }
    }
    
    if (normal.dot(vmin) > 0.0f) {
        return false;
    } else if (normal.dot(vmax) >= 0.0f) {
        return true;
    } else {
        return false;
    }
}

/*======================== X-tests ========================*/

#define AXISTEST_X01(a, b, fa, fb)   \
    p0 = a*v0[Y] - b*v0[Z];          \
    p2 = a*v2[Y] - b*v2[Z];          \
    if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
    rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z];   \
    if(min>rad || max<-rad) return false;


#define AXISTEST_X2(a, b, fa, fb)    \
    p0 = a*v0[Y] - b*v0[Z];          \
    p1 = a*v1[Y] - b*v1[Z];          \
    if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
    rad = fa * boxhalfsize[Y] + fb * boxhalfsize[Z];   \
    if(min>rad || max<-rad) return false;

/*======================== Y-tests ========================*/

#define AXISTEST_Y02(a, b, fa, fb)   \
    p0 = -a*v0[X] + b*v0[Z];         \
    p2 = -a*v2[X] + b*v2[Z];         \
    if(p0<p2) {min=p0; max=p2;} else {min=p2; max=p0;} \
    rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z];   \
    if(min>rad || max<-rad) return false;

#define AXISTEST_Y1(a, b, fa, fb)   \
    p0 = -a*v0[X] + b*v0[Z];        \
    p1 = -a*v1[X] + b*v1[Z];        \
    if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
    rad = fa * boxhalfsize[X] + fb * boxhalfsize[Z];   \
    if(min>rad || max<-rad) return false;

/*======================== Z-tests ========================*/

#define AXISTEST_Z12(a, b, fa, fb)   \
    p1 = a*v1[X] - b*v1[Y];          \
    p2 = a*v2[X] - b*v2[Y];          \
    if(p2<p1) {min=p2; max=p1;} else {min=p1; max=p2;} \
    rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y];   \
    if(min>rad || max<-rad) return false;

#define AXISTEST_Z0(a, b, fa, fb)   \
    p0 = a*v0[X] - b*v0[Y];   \
    p1 = a*v1[X] - b*v1[Y];           \
    if(p0<p1) {min=p0; max=p1;} else {min=p1; max=p0;} \
    rad = fa * boxhalfsize[X] + fb * boxhalfsize[Y];   \
    if(min>rad || max<-rad) return false;

bool CollisionDetection::fixedSolidBoxIntersectsFixedTriangle(
   const AABox& box, const Triangle& tri) {

    //    use separating axis theorem to test overlap between triangle and box 
    //    need to test for overlap in these directions: 
    //    1) the {x,y,z}-directions (actually, since we use the AABB of the triangle 
    //       we do not even need to test these) 
    //    2) normal of the triangle 
    //    3) crossproduct(edge from tri, {x,y,z}-direction) 
    //       this gives 3x3=9 more tests 

    // This is the fastest branch (on Sun).
    // Move the triangle to the object space of the box
    // Triangle vertices in box object space

    const Vector3& boxcenter = box.center();
    const Vector3& boxhalfsize = box.extent() * 0.5f;

    const Vector3& v0 = tri.vertex(0) - boxcenter;
    const Vector3& v1 = tri.vertex(1) - boxcenter;
    const Vector3& v2 = tri.vertex(2) - boxcenter;

    // Compute triangle edges in object space
    const Vector3& e0 = v1 - v0;
    const Vector3& e1 = v2 - v1;
    const Vector3& e2 = v0 - v2;

    // Bullet 3: 
    //  test the 9 tests first (this was faster) 
    float min,max,p0,p1,p2,rad;
    Vector3 fe;

    fe = abs(e0);
    AXISTEST_X01(e0[Z], e0[Y], fe[Z], fe[Y]);
    AXISTEST_Y02(e0[Z], e0[X], fe[Z], fe[X]);
    AXISTEST_Z12(e0[Y], e0[X], fe[Y], fe[X]);
    
    fe = abs(e1);
    AXISTEST_X01(e1[Z], e1[Y], fe[Z], fe[Y]);
    AXISTEST_Y02(e1[Z], e1[X], fe[Z], fe[X]);
    AXISTEST_Z0 (e1[Y], e1[X], fe[Y], fe[X]);

    fe = abs(e2);
    AXISTEST_X2 (e2[Z], e2[Y], fe[Z], fe[Y]);
    AXISTEST_Y1 (e2[Z], e2[X], fe[Z], fe[X]);
    AXISTEST_Z12(e2[Y], e2[X], fe[Y], fe[X]);

    // Bullet 1: 
    //  first test overlap in the {x,y,z}-directions 
    //  find min, max of the triangle each direction, and test for overlap in 
    //  that direction -- this is equivalent to testing a minimal AABB around
    //  the triangle against the AABB 

    // test in X-direction 
    FINDMINMAX(v0[X],v1[X],v2[X],min,max);
    if (min > boxhalfsize[X] || max < -boxhalfsize[X]) {
        return false;
    }

    // test in Y-direction
    FINDMINMAX(v0[Y],v1[Y],v2[Y],min,max);
    if (min > boxhalfsize[Y] || max < -boxhalfsize[Y]) {
        return false;
    }

    // test in Z-direction 
    FINDMINMAX(v0[Z],v1[Z],v2[Z],min,max);
    if (min > boxhalfsize[Z] || max < -boxhalfsize[Z]) {
        return false;
    }

    // Bullet 2: 
    //  test if the box intersects the plane of the triangle 
    //  compute plane equation of triangle: normal*x+d=0 

    if (! planeBoxOverlap(tri.normal(), v0, boxhalfsize)) {
        return false;
    }

    // box and triangle overlap
    return true;
}
#undef X
#undef Y
#undef Z

////////////////////////////////////////////////////////////////////////////////


} // namespace

#ifdef _MSC_VER
// Turn off fast floating-point optimizations
#pragma float_control( pop )
#pragma warning (pop)
#endif