Core/Dependency: Upgrade G3d-library to v8.0-release (patched version!)

Note: Due to issues with G3D (normally requiring X11 and the ZIP-library),
the sourcetree version contains a modified version. The applied patch is
commited to the repository for future reference.

--HG--
branch : trunk

1 files changed, 2455 insertions, 0 deletions

diff --git a/dep/g3dlite/source/CollisionDetection.cpp b/dep/g3dlite/source/CollisionDetection.cpp
new file mode 100644
index 00000000000..77eef0a5500
--- /dev/null
+++ b/dep/g3dlite/source/CollisionDetection.cpp
@@ -0,0 +1,2455 @@
+/**
+  @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;
+        const Box* faceBox;
+        Vector3 faceNormal = L;
+
+        // L will be the outward facing normal for the faceBox
+        if (penetrationAxisIndex < 3) {
+            faceBox = & box1;
+            vertexBox = & box2;
+            if (dot(L, D) < 0) {
+                faceNormal = -L;
+            }
+        } else {
+            faceBox = & box2;
+            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.
+    double d;
+    Vector3 normal;
+    plane.getEquation(normal, d);
+    
+    float vdotN = velocity.dot(normal);
+    float pdotN = point.dot(normal);
+
+    if (fuzzyEq(pdotN + d, 0)) {
+        // The point is *in* the plane.
+        location = point;
+        outNormal = normal;
+        return 0;
+    }
+
+    if (vdotN >= 0) {
+        // no collision will occur
+        location = Vector3::inf();
+        return finf();
+    }
+
+    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 kU, kV, kW = capsuleDirection;
+    float fWLength = kW.unitize();
+    Vector3::generateOrthonormalBasis(kU, kV, kW);
+    Vector3 kD(kU.dot(rkDirection), kV.dot(rkDirection), kW.dot(rkDirection));
+    float fDLength = kD.unitize();
+
+    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 collision point on the sphere will be the point at
+    // center - (radius * normal).  Collisions only occur when
+    // the sphere is travelling into the plane.
+
+    double d;
+    plane.getEquation(outNormal, d);
+    
+    double vdotN = velocity.dot(outNormal);
+
+    if (fuzzyGt(vdotN, 0)) {
+        // No collision when the sphere is moving towards a backface.
+        location = Vector3::inf();
+        return (float)finf();
+    }
+
+    float cdotN = sphere.center.dot(outNormal);
+
+    // Distance from the center to the plane
+    float distance = cdotN + (float)d;
+
+    // Where is the collision on the sphere?
+    Vector3 point = sphere.center - (sphere.radius * outNormal);
+
+    if (fuzzyLe(G3D::abs(distance), sphere.radius)) {
+        // Already interpenetrating
+        location = sphere.center - distance * outNormal;
+        return 0;
+    } else {
+        return collisionTimeForMovingPointFixedPlane(point, velocity, plane, location, outNormal);
+    }
+
+}
+
+
+float CollisionDetection::collisionTimeForMovingSphereFixedTriangle(
+    const class Sphere&		sphere,
+    const Vector3&              velocity,
+    const Triangle&             triangle,
+    Vector3&                    outLocation,
+    float                       b[3]) {
+
+    Vector3 dummy;
+    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.");
+            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.unitize();
+    }
+
+    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