///<summary>
        /// Constructs a new transformable shape.
        ///</summary>
        /// <param name="shape">Base shape to transform.</param>
        /// <param name="transform">Transform to use.</param>
        /// <param name="description">Cached information about the shape. Assumed to be correct; no extra processing or validation is performed.</param>
        public TransformableShape(ConvexShape shape, Matrix3x3 transform, ConvexShapeDescription description)
        {
            this.shape = shape;
            this.transform = transform;

            UpdateConvexShapeInfo(description);
        }

 public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, Func<BroadPhaseEntry, bool> filter, out RayHit hit)
 {
     Vector3 swp = sweep;
     double len = swp.Length();
     swp /= len;
     return ConvexCast(castShape, ref startingTransform, ref swp, len, MaterialSolidity.FULLSOLID, out hit);
 }

 ///<summary>
 /// Computes the center and volume of a convex shape.
 ///</summary>
 ///<param name="shape">Shape to compute the center of.</param>
 ///<param name="volume">Volume of the shape.</param>
 ///<returns>Center of the shape.</returns>
 public static Vector3 ComputeCenter(ConvexShape shape, out float volume)
 {
     var pointContributions = Resources.GetVectorList();
     GetPoints(shape, out volume, pointContributions);
     Vector3 center = AveragePoints(pointContributions);
     Resources.GiveBack(pointContributions);
     return center;
 }

 ///<summary>
 /// Computes the volume and volume distribution of a shape.
 ///</summary>
 ///<param name="shape">Shape to compute the volume information of.</param>
 ///<param name="volume">Volume of the shape.</param>
 ///<returns>Volume distribution of the shape.</returns>
 public static Matrix3x3 ComputeVolumeDistribution(ConvexShape shape, out float volume)
 {
     var pointContributions = CommonResources.GetVectorList();
     GetPoints(shape, out volume, pointContributions);
     Vector3 center = AveragePoints(pointContributions);
     Matrix3x3 volumeDistribution = ComputeVolumeDistribution(pointContributions, ref center);
     CommonResources.GiveBack(pointContributions);
     return volumeDistribution;
 }

        ///<summary>
        /// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA.
        ///</summary>
        ///<param name="shapeA">First shape.</param>
        ///<param name="shapeB">Second shape.</param>
        ///<param name="direction">Extreme point direction in local space.</param>
        ///<param name="localTransformB">Transform of shapeB in the local space of A.</param>
        ///<param name="extremePoint">The extreme point in the local space of A.</param>
        public static void GetLocalMinkowskiExtremePoint(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 direction, ref RigidTransform localTransformB, out Vector3 extremePoint)
        {
            //Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D)
            shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePoint);
            Vector3 v;
            Vector3 negativeN;
            Vector3.Negate(ref direction, out negativeN);
            shapeB.GetExtremePointWithoutMargin(negativeN, ref localTransformB, out v);
            Vector3.Subtract(ref extremePoint, ref v, out extremePoint);

            ExpandMinkowskiSum(shapeA.collisionMargin, shapeB.collisionMargin, ref direction, out v);
            Vector3.Add(ref extremePoint, ref v, out extremePoint);
        }

 public bool SpecialCaseConvexTrace(ConvexShape shape, Location start, Location dir, double len, MaterialSolidity considerSolid, Func<BroadPhaseEntry, bool> filter, out RayCastResult rayHit)
 {
     RigidTransform rt = new RigidTransform(start.ToBVector(), BEPUutilities.Quaternion.Identity);
     BEPUutilities.Vector3 sweep = (dir * len).ToBVector();
     RayCastResult best = new RayCastResult(new RayHit() { T = len }, null);
     bool hA = false;
     if (considerSolid.HasFlag(MaterialSolidity.FULLSOLID))
     {
         RayCastResult rcr;
         if (PhysicsWorld.ConvexCast(shape, ref rt, ref sweep, filter, out rcr))
         {
             best = rcr;
             hA = true;
         }
     }
     sweep = dir.ToBVector();
     AABB box = new AABB();
     box.Min = start;
     box.Max = start;
     box.Include(start + dir * len);
     foreach (KeyValuePair<Vector3i, Chunk> chunk in LoadedChunks)
     {
         if (chunk.Value == null || chunk.Value.FCO == null)
         {
             continue;
         }
         if (!box.Intersects(new AABB() { Min = chunk.Value.WorldPosition.ToLocation() * Chunk.CHUNK_SIZE,
             Max = chunk.Value.WorldPosition.ToLocation() * Chunk.CHUNK_SIZE + new Location(Chunk.CHUNK_SIZE, Chunk.CHUNK_SIZE, Chunk.CHUNK_SIZE) }))
         {
             continue;
         }
         RayHit temp;
         if (chunk.Value.FCO.ConvexCast(shape, ref rt, ref sweep, len, considerSolid, out temp))
         {
             hA = true;
             if (temp.T < best.HitData.T)
             {
                 best.HitData = temp;
                 best.HitObject = chunk.Value.FCO;
             }
         }
     }
     rayHit = best;
     return hA;
 }

        ///<summary>
        /// Tests if the pair is intersecting.
        ///</summary>
        ///<param name="shapeA">First shape of the pair.</param>
        ///<param name="shapeB">Second shape of the pair.</param>
        ///<param name="transformA">Transform to apply to the first shape.</param>
        ///<param name="transformB">Transform to apply to the second shape.</param>
        ///<param name="localSeparatingAxis">Warmstartable separating axis used by the method to quickly early-out if possible.  Updated to the latest separating axis after each run.</param>
        ///<returns>Whether or not the objects were intersecting.</returns>
        public static bool AreShapesIntersecting(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB,
                                                 ref Vector3 localSeparatingAxis)
        {
            RigidTransform localtransformB;
            MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB);

            //Warm start the simplex.
            var simplex = new SimpleSimplex();
            Vector3 extremePoint;
            MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref localSeparatingAxis, ref localtransformB, out extremePoint);
            simplex.AddNewSimplexPoint(ref extremePoint);

            Vector3 closestPoint;
            int count = 0;
            while (count++ < MaximumGJKIterations)
            {
                if (simplex.GetPointClosestToOrigin(out closestPoint) || //Also reduces the simplex.
                    closestPoint.LengthSquared() <= simplex.GetErrorTolerance() * Toolbox.BigEpsilon)
                {
                    //Intersecting, or so close to it that it will be difficult/expensive to figure out the separation.
                    return true;
                }

                //Use the closest point as a direction.
                Vector3 direction;
                Vector3.Negate(ref closestPoint, out direction);
                MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref direction, ref localtransformB, out extremePoint);
                //Since this is a boolean test, we don't need to refine the simplex if it becomes apparent that we cannot reach the origin.
                //If the most extreme point at any given time does not go past the origin, then we can quit immediately.
                float dot;
                Vector3.Dot(ref extremePoint, ref closestPoint, out dot); //extreme point dotted against the direction pointing backwards towards the CSO. 
                if (dot > 0)
                {
                    // If it's positive, that means that the direction pointing towards the origin produced an extreme point 'in front of' the origin, eliminating the possibility of any intersection.
                    localSeparatingAxis = direction;
                    return false;
                }

                simplex.AddNewSimplexPoint(ref extremePoint);


            }
            return false;
        }

        //TODO: Consider changing the termination epsilons on these casts.  Epsilon * Modifier is okay, but there might be better options.

        ///<summary>
        /// Tests a ray against a convex shape.
        ///</summary>
        ///<param name="ray">Ray to test against the shape.</param>
        ///<param name="shape">Shape to test.</param>
        ///<param name="shapeTransform">Transform to apply to the shape for the test.</param>
        ///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
        ///<param name="hit">Hit data of the ray cast, if any.</param>
        ///<returns>Whether or not the ray hit the shape.</returns>
        public static bool RayCast(Ray ray, ConvexShape shape, ref RigidTransform shapeTransform, float maximumLength,
                                   out RayHit hit)
        {
            //Transform the ray into the object's local space.
            Vector3.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position);
            Quaternion conjugate;
            Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate);
            Quaternion.Transform(ref ray.Position, ref conjugate, out ray.Position);
            Quaternion.Transform(ref ray.Direction, ref conjugate, out ray.Direction);

            Vector3 extremePointToRayOrigin, extremePoint;
            hit.T = 0;
            hit.Location = ray.Position;
            hit.Normal = Toolbox.ZeroVector;
            Vector3 closestOffset = hit.Location;

            RaySimplex simplex = new RaySimplex();

            float vw, closestPointDotDirection;
            int count = 0;
            //This epsilon has a significant impact on performance and accuracy.  Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident.
            while (closestOffset.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref ray.Position))
            {
                if (++count > MaximumGJKIterations)
                {
                    //It's taken too long to find a hit.  Numerical problems are probable; quit.
                    hit = new RayHit();
                    return false;
                }

                shape.GetLocalExtremePoint(closestOffset, out extremePoint);

                Vector3.Subtract(ref hit.Location, ref extremePoint, out extremePointToRayOrigin);
                Vector3.Dot(ref closestOffset, ref extremePointToRayOrigin, out vw);
                //If the closest offset and the extreme point->ray origin direction point the same way,
                //then we might be able to conservatively advance the point towards the surface.
                if (vw > 0)
                {
                    
                    Vector3.Dot(ref closestOffset, ref ray.Direction, out closestPointDotDirection);
                    if (closestPointDotDirection >= 0)
                    {
                        hit = new RayHit();
                        return false;
                    }
                    hit.T = hit.T - vw / closestPointDotDirection;
                    if (hit.T > maximumLength)
                    {
                        //If we've gone beyond where the ray can reach, there's obviously no hit.
                        hit = new RayHit();
                        return false;
                    }
                    //Shift the ray up.
                    Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
                    Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
                    hit.Normal = closestOffset;
                }

                RaySimplex shiftedSimplex;
                simplex.AddNewSimplexPoint(ref extremePoint, ref hit.Location, out shiftedSimplex);

                //Compute the offset from the simplex surface to the origin.
                shiftedSimplex.GetPointClosestToOrigin(ref simplex, out closestOffset);

            }
            //Transform the hit data into world space.
            Quaternion.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal);
            Quaternion.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location);
            Vector3.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location);

            return true;
        }

        private static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform localTransformB,
                                             ref CachedSimplex cachedSimplex, out Vector3 localClosestPointA, out Vector3 localClosestPointB)
        {

            var simplex = new PairSimplex(ref cachedSimplex, ref localTransformB);

            Vector3 closestPoint;
            int count = 0;
            while (true)
            {
                if (simplex.GetPointClosestToOrigin(out closestPoint) || //Also reduces the simplex and computes barycentric coordinates if necessary. 
                    closestPoint.LengthSquared() <= Toolbox.Epsilon * simplex.errorTolerance)
                {
                    //Intersecting.
                    localClosestPointA = Toolbox.ZeroVector;
                    localClosestPointB = Toolbox.ZeroVector;

                    simplex.UpdateCachedSimplex(ref cachedSimplex);
                    return true;
                }

                if (++count > MaximumGJKIterations)
                    break; //Must break BEFORE a new vertex is added if we're over the iteration limit.  This guarantees final simplex is not a tetrahedron.

                if (simplex.GetNewSimplexPoint(shapeA, shapeB, count, ref closestPoint))
                {
                    //No progress towards origin, not intersecting.
                    break;
                }

            }
            //Compute closest points from the contributing simplexes and barycentric coordinates
            simplex.GetClosestPoints(out localClosestPointA, out localClosestPointB);
            //simplex.VerifyContributions();
            //if (Vector3.Distance(localClosestPointA - localClosestPointB, closestPoint) > .00001f)
            //    Debug.WriteLine("break.");
            simplex.UpdateCachedSimplex(ref cachedSimplex);
            return false;
        }

 public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
 {
     hit = new RayHit();
     BoundingBox boundingBox;
     castShape.GetSweptBoundingBox(ref startingTransform, ref sweep, out boundingBox);
     var tri = PhysicsResources.GetTriangle();
     var hitElements = CommonResources.GetIntList();
     if (triangleMesh.Tree.GetOverlaps(boundingBox, hitElements))
     {
         hit.T = float.MaxValue;
         for (int i = 0; i < hitElements.Count; i++)
         {
             triangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC);
             Vector3 center;
             Vector3.Add(ref tri.vA, ref tri.vB, out center);
             Vector3.Add(ref center, ref tri.vC, out center);
             Vector3.Multiply(ref center, 1f / 3f, out center);
             Vector3.Subtract(ref tri.vA, ref center, out tri.vA);
             Vector3.Subtract(ref tri.vB, ref center, out tri.vB);
             Vector3.Subtract(ref tri.vC, ref center, out tri.vC);
             tri.maximumRadius = tri.vA.LengthSquared();
             float radius = tri.vB.LengthSquared();
             if (tri.maximumRadius < radius)
                 tri.maximumRadius = radius;
             radius = tri.vC.LengthSquared();
             if (tri.maximumRadius < radius)
                 tri.maximumRadius = radius;
             tri.maximumRadius = (float)Math.Sqrt(tri.maximumRadius);
             tri.collisionMargin = 0;
             var triangleTransform = new RigidTransform { Orientation = Quaternion.Identity, Position = center };
             RayHit tempHit;
             if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T)
             {
                 hit = tempHit;
             }
         }
         tri.maximumRadius = 0;
         PhysicsResources.GiveBack(tri);
         CommonResources.GiveBack(hitElements);
         return hit.T != float.MaxValue;
     }
     PhysicsResources.GiveBack(tri);
     CommonResources.GiveBack(hitElements);
     return false;
 }

 /// <summary>
 /// Sweeps a convex shape against the entry.
 /// </summary>
 /// <param name="castShape">Swept shape.</param>
 /// <param name="startingTransform">Beginning location and orientation of the cast shape.</param>
 /// <param name="sweep">Sweep motion to apply to the cast shape.</param>
 /// <param name="hit">Hit data of the ray on the entry, if any.</param>
 /// <returns>Whether or not the ray hit the entry.</returns>
 public abstract bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit);

 /// <summary>
 /// Sweeps a convex shape against the entry.
 /// </summary>
 /// <param name="castShape">Swept shape.</param>
 /// <param name="startingTransform">Beginning location and orientation of the cast shape.</param>
 /// <param name="sweep">Sweep motion to apply to the cast shape.</param>
 /// <param name="filter">Test to apply to the entry. If it returns true, the entry is processed, otherwise the entry is ignored. If a collidable hierarchy is present
 /// in the entry, this filter will be passed into inner ray casts.</param>
 /// <param name="hit">Hit data of the cast on the entry, if any.</param>
 /// <returns>Whether or not the cast hit the entry.</returns>
 public virtual bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, Func<BroadPhaseEntry, bool> filter, out RayHit hit)
 {
     if (filter(this))
         return ConvexCast(castShape, ref startingTransform, ref sweep, out hit);
     hit = new RayHit();
     return false;
 }

 ///<summary>
 /// Computes the center of a convex shape.
 ///</summary>
 ///<param name="shape">Shape to compute the center of.</param>
 ///<returns>Center of the shape.</returns>
 public static Vector3 ComputeCenter(ConvexShape shape)
 {
     float volume;
     return ComputeCenter(shape, out volume);
 }

        ///<summary>
        /// Gets the point contributions within a convex shape.
        ///</summary>
        ///<param name="shape">Shape to compute the point contributions of.</param>
        ///<param name="volume">Volume of the shape.</param>
        ///<param name="outputPointContributions">Point contributions of the shape.</param>
        public static void GetPoints(ConvexShape shape, out float volume, RawList<Vector3> outputPointContributions)
        {
            RigidTransform transform = RigidTransform.Identity;
            BoundingBox boundingBox;
            shape.GetBoundingBox(ref transform, out boundingBox);

            //Find the direction which maximizes the possible hits.  Generally, this is the smallest area axis.
            //Possible options are:
            //YZ -> use X
            //XZ -> use Y
            //XY -> use Z
            Ray ray;
            float width = boundingBox.Max.X - boundingBox.Min.X;
            float height = boundingBox.Max.Y - boundingBox.Min.Y;
            float length = boundingBox.Max.Z - boundingBox.Min.Z;
            float yzArea = height * length;
            float xzArea = width * length;
            float xyArea = width * height;
            Vector3 increment1, increment2;
            float incrementMultiplier = 1f / NumberOfSamplesPerDimension;
            float maxLength;
            float rayIncrement;
            if (yzArea > xzArea && yzArea > xyArea)
            {
                //use the x axis as the direction.
                ray.Direction = Vector3.Right;
                ray.Position = new Vector3(boundingBox.Min.X, boundingBox.Min.Y + .5f * incrementMultiplier * height, boundingBox.Min.Z + .5f * incrementMultiplier * length);
                increment1 = new Vector3(0, incrementMultiplier * height, 0);
                increment2 = new Vector3(0, 0, incrementMultiplier * length);
                rayIncrement = incrementMultiplier * width;
                maxLength = width;
            }
            else if (xzArea > xyArea) //yz is not the max, given by the previous if.  Is xz or xy the max?
            {
                //use the y axis as the direction.
                ray.Direction = Vector3.Up;
                ray.Position = new Vector3(boundingBox.Min.X + .5f * incrementMultiplier * width, boundingBox.Min.Y, boundingBox.Min.Z + .5f * incrementMultiplier * length);
                increment1 = new Vector3(incrementMultiplier * width, 0, 0);
                increment2 = new Vector3(0, 0, incrementMultiplier * height);
                rayIncrement = incrementMultiplier * height;
                maxLength = height;
            }
            else
            {
                //use the z axis as the direction.
                ray.Direction = Vector3.Backward;
                ray.Position = new Vector3(boundingBox.Min.X + .5f * incrementMultiplier * width, boundingBox.Min.Y + .5f * incrementMultiplier * height, boundingBox.Min.Z);
                increment1 = new Vector3(incrementMultiplier * width, 0, 0);
                increment2 = new Vector3(0, incrementMultiplier * height, 0);
                rayIncrement = incrementMultiplier * length;
                maxLength = length;
            }


            Ray oppositeRay;
            volume = 0;
            for (int i = 0; i < NumberOfSamplesPerDimension; i++)
            {
                for (int j = 0; j < NumberOfSamplesPerDimension; j++)
                {
                    //Ray cast from one direction.  If it succeeds, try the other way.  This forms an interval in which inertia tensor contributions are contained.
                    RayHit hit;
                    if (shape.RayTest(ref ray, ref transform, maxLength, out hit))
                    {
                        Vector3.Multiply(ref ray.Direction, maxLength, out oppositeRay.Position);
                        Vector3.Add(ref oppositeRay.Position, ref ray.Position, out oppositeRay.Position);
                        Vector3.Negate(ref ray.Direction, out oppositeRay.Direction);
                        RayHit oppositeHit;
                        if (shape.RayTest(ref oppositeRay, ref transform, maxLength, out oppositeHit))
                        {
                            //It should always get here if one direction casts, but there may be numerical issues.
                            float scanVolume;
                            ScanObject(rayIncrement, maxLength, ref increment1, ref increment2, ref ray, ref hit, ref oppositeHit, outputPointContributions, out scanVolume);
                            volume += scanVolume;
                        }
                    }
                    Vector3.Add(ref ray.Position, ref increment2, out ray.Position);
                }
                Vector3.Add(ref ray.Position, ref increment1, out ray.Position);
                //Move the ray back to the starting position along the other axis.
                Vector3 subtract;
                Vector3.Multiply(ref increment2, NumberOfSamplesPerDimension, out subtract);
                Vector3.Subtract(ref ray.Position, ref subtract, out ray.Position);
            }


        }

 ///<summary>
 /// Constructs a new entry with identity orientation.
 ///</summary>
 ///<param name="shape">Shape of the entry.</param>
 public OrientedConvexShapeEntry(ConvexShape shape)
 {
     Orientation = Quaternion.Identity;
     CollisionShape = shape;
 }

 ///<summary>
 /// Sweeps a shape against another shape using a given sweep vector.
 ///</summary>
 ///<param name="sweptShape">Shape to sweep.</param>
 ///<param name="target">Shape being swept against.</param>
 ///<param name="sweep">Sweep vector for the sweptShape.</param>
 ///<param name="startingSweptTransform">Starting transform of the sweptShape.</param>
 ///<param name="targetTransform">Transform to apply to the target shape.</param>
 ///<param name="hit">Hit data of the sweep test, if any.</param>
 ///<returns>Whether or not the swept shape hit the other shape.</returns>
 public static bool ConvexCast(ConvexShape sweptShape, ConvexShape target, ref Vector3 sweep, ref RigidTransform startingSweptTransform, ref RigidTransform targetTransform,
                           out RayHit hit)
 {
     return ConvexCast(sweptShape, target, ref sweep, ref Toolbox.ZeroVector, ref startingSweptTransform, ref targetTransform, out hit);
 }

        ///<summary>
        /// Constructs a new transformable shape.
        ///</summary>
        ///<param name="shape">Base shape to transform.</param>
        ///<param name="transform">Transform to use.</param>
        public TransformableShape(ConvexShape shape, Matrix3X3 transform)
        {
            this.shape = shape;
            Transform = transform;

        }

        ///<summary>
        /// Casts a fat (sphere expanded) ray against the shape.
        ///</summary>
        ///<param name="ray">Ray to test against the shape.</param>
        ///<param name="radius">Radius of the ray.</param>
        ///<param name="shape">Shape to test against.</param>
        ///<param name="shapeTransform">Transform to apply to the shape for the test.</param>
        ///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
        ///<param name="hit">Hit data of the sphere cast, if any.</param>
        ///<returns>Whether or not the sphere cast hit the shape.</returns>
        public static bool SphereCast(Ray ray, float radius, ConvexShape shape, ref RigidTransform shapeTransform, float maximumLength,
                                   out RayHit hit)
        {
            //Transform the ray into the object's local space.
            Vector3.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position);
            Quaternion conjugate;
            Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate);
            Quaternion.Transform(ref ray.Position, ref conjugate, out ray.Position);
            Quaternion.Transform(ref ray.Direction, ref conjugate, out ray.Direction);

            Vector3 w, p;
            hit.T = 0;
            hit.Location = ray.Position;
            hit.Normal = Toolbox.ZeroVector;
            Vector3 v = hit.Location;

            RaySimplex simplex = new RaySimplex();

            float vw, vdir;
            int count = 0;

            //This epsilon has a significant impact on performance and accuracy.  Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident.
            while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref ray.Position))
            {
                if (++count > MaximumGJKIterations)
                {
                    //It's taken too long to find a hit.  Numerical problems are probable; quit.
                    hit = new RayHit();
                    return false;
                }

                shape.GetLocalExtremePointWithoutMargin(ref v, out p);
                Vector3 contribution;
                MinkowskiToolbox.ExpandMinkowskiSum(shape.collisionMargin, radius, ref v, out contribution);
                Vector3.Add(ref p, ref contribution, out p);

                Vector3.Subtract(ref hit.Location, ref p, out w);
                Vector3.Dot(ref v, ref w, out vw);
                if (vw > 0)
                {
                    Vector3.Dot(ref v, ref ray.Direction, out vdir);
                    hit.T = hit.T - vw / vdir;
                    if (vdir >= 0)
                    {
                        //We would have to back up!
                        return false;
                    }
                    if (hit.T > maximumLength)
                    {
                        //If we've gone beyond where the ray can reach, there's obviously no hit.
                        return false;
                    }
                    //Shift the ray up.
                    Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
                    Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location);
                    hit.Normal = v;
                }

                RaySimplex shiftedSimplex;
                simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex);

                shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v);

            }
            //Transform the hit data into world space.
            Quaternion.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal);
            Quaternion.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location);
            Vector3.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location);

            return true;
        }

        /// <summary>
        /// Casts a convex shape against the collidable.
        /// </summary>
        /// <param name="castShape">Shape to cast.</param>
        /// <param name="startingTransform">Initial transform of the shape.</param>
        /// <param name="sweep">Sweep to apply to the shape.</param>
        /// <param name="hit">Hit data, if any.</param>
        /// <returns>Whether or not the cast hit anything.</returns>
        public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
        {
            if (Shape.solidity == MobileMeshSolidity.Solid)
            {
                //If the convex cast is inside the mesh and the mesh is solid, it should return t = 0.
                var ray = new Ray() { Position = startingTransform.Position, Direction = Toolbox.UpVector };
                if (Shape.IsLocalRayOriginInMesh(ref ray, out hit))
                {

                    hit = new RayHit() { Location = startingTransform.Position, Normal = new Vector3(), T = 0 };
                    return true;
                }
            }
            hit = new RayHit();
            BoundingBox boundingBox;
            var transform = new AffineTransform {Translation = worldTransform.Position};
            Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out transform.LinearTransform);
            castShape.GetSweptLocalBoundingBox(ref startingTransform, ref transform, ref sweep, out boundingBox);
            var tri = PhysicsResources.GetTriangle();
            var hitElements = CommonResources.GetIntList();
            if (this.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, hitElements))
            {
                hit.T = float.MaxValue;
                for (int i = 0; i < hitElements.Count; i++)
                {
                    Shape.TriangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC);
                    AffineTransform.Transform(ref tri.vA, ref transform, out tri.vA);
                    AffineTransform.Transform(ref tri.vB, ref transform, out tri.vB);
                    AffineTransform.Transform(ref tri.vC, ref transform, out tri.vC);
                    Vector3 center;
                    Vector3.Add(ref tri.vA, ref tri.vB, out center);
                    Vector3.Add(ref center, ref tri.vC, out center);
                    Vector3.Multiply(ref center, 1f / 3f, out center);
                    Vector3.Subtract(ref tri.vA, ref center, out tri.vA);
                    Vector3.Subtract(ref tri.vB, ref center, out tri.vB);
                    Vector3.Subtract(ref tri.vC, ref center, out tri.vC);
                    tri.maximumRadius = tri.vA.LengthSquared();
                    float radius = tri.vB.LengthSquared();
                    if (tri.maximumRadius < radius)
                        tri.maximumRadius = radius;
                    radius = tri.vC.LengthSquared();
                    if (tri.maximumRadius < radius)
                        tri.maximumRadius = radius;
                    tri.maximumRadius = (float)Math.Sqrt(tri.maximumRadius);
                    tri.collisionMargin = 0;
                    var triangleTransform = new RigidTransform {Orientation = Quaternion.Identity, Position = center};
                    RayHit tempHit;
                    if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T)
                    {
                        hit = tempHit;
                    }
                }
                tri.maximumRadius = 0;
                PhysicsResources.GiveBack(tri);
                CommonResources.GiveBack(hitElements);
                return hit.T != float.MaxValue;
            }
            PhysicsResources.GiveBack(tri);
            CommonResources.GiveBack(hitElements);
            return false;
        }

		///<summary>
		/// Casts a fat (sphere expanded) ray against the shape.  If the raycast appears to be stuck in the shape, the cast will be attempted
		/// with a smaller ray (scaled by the MotionSettings.CoreShapeScaling each time).
		///</summary>
		///<param name="ray">Ray to test against the shape.</param>
		///<param name="radius">Radius of the ray.</param>
		///<param name="target">Shape to test against.</param>
		///<param name="shapeTransform">Transform to apply to the shape for the test.</param>
		///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
		///<param name="hit">Hit data of the sphere cast, if any.</param>
		///<returns>Whether or not the sphere cast hit the shape.</returns>
		public static bool CCDSphereCast( ref Ray ray, float radius, ConvexShape target, ref RigidTransform shapeTransform, float maximumLength,
								   out RayHit hit )
		{
			int iterations = 0;
			while( true )
			{
				if( GJKToolbox.SphereCast( ray, radius, target, ref shapeTransform, maximumLength, out hit ) &&
					hit.T > 0 )
				{
					//The ray cast isn't embedded in the shape, and it's less than maximum length away!
					return true;
				}
				if( hit.T > maximumLength || hit.T < 0 )
					return false; //Failure showed it was too far, or behind.

				radius *= MotionSettings.CoreShapeScaling;
				iterations++;
				if( iterations > 3 ) //Limit could be configurable.
				{
					//It's iterated too much, let