17 files changed, 3346 insertions, 3346 deletions

diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs
index 497f220..d011ef2 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs
@@ -1,199 +1,199 @@
-/*

-  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru

-  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

-

-  This software is provided 'as-is', without any express or implied

-  warranty.  In no event will the authors be held liable for any damages

-  arising from the use of this software.

-

-  Permission is granted to anyone to use this software for any purpose,

-  including commercial applications, and to alter it and redistribute it

-  freely, subject to the following restrictions:

-

-  1. The origin of this software must not be misrepresented; you must not

-     claim that you wrote the original software. If you use this software

-     in a product, an acknowledgment in the product documentation would be

-     appreciated but is not required.

-  2. Altered source versions must be plainly marked as such, and must not be

-     misrepresented as being the original software.

-  3. This notice may not be removed or altered from any source distribution.

-*/

-

-using System;

-using System.Collections.Generic;

-using System.Text;

-using MonoXnaCompactMaths;

-

-namespace XnaDevRu.BulletX

-{

-        /// <summary>

-        /// ContinuousConvexCollision implements angular and linear time of impact for convex objects.

-        /// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis).

-        /// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent.

-        /// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops

-        /// </summary>

-        public class ContinuousConvexCollision : IConvexCast

-        {

-                /// <summary>

-                /// This maximum should not be necessary. It allows for untested/degenerate cases in production code.

-                /// You don't want your game ever to lock-up.

-                /// </summary>

-                private const int MaxIterations = 1000;

-

-                private ISimplexSolver _simplexSolver;

-                private IConvexPenetrationDepthSolver _penetrationDepthSolver;

-                private ConvexShape _convexA;

-                private ConvexShape _convexB;

-

-                public ContinuousConvexCollision(ConvexShape convexA, ConvexShape convexB,

-                        ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)

-                {

-                        _simplexSolver = simplexSolver;

-                        _penetrationDepthSolver = penetrationDepthSolver;

-                        _convexA = convexA;

-                        _convexB = convexB;

-                }

-

-                public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)

-                {

-                        _simplexSolver.Reset();

-

-                        // compute linear and angular velocity for this interval, to interpolate

-                        Vector3 linVelA = new Vector3(), angVelA = new Vector3(), linVelB = new Vector3(), angVelB = new Vector3();

-                        TransformUtil.CalculateVelocity(fromA, toA, 1f, ref linVelA, ref angVelA);

-                        TransformUtil.CalculateVelocity(fromB, toB, 1f, ref linVelB, ref angVelB);

-

-                        float boundingRadiusA = _convexA.GetAngularMotionDisc();

-                        float boundingRadiusB = _convexB.GetAngularMotionDisc();

-

-                        float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA +

-                                angVelB.Length() * boundingRadiusB;

-

-                        float radius = 0.001f;

-

-                        float lambda = 0f;

-                        Vector3 v = new Vector3(1f, 0f, 0f);

-

-                        int maxIter = MaxIterations;

-

-                        Vector3 n = new Vector3();

-                        bool hasResult = false;

-                        Vector3 c;

-

-                        float lastLambda = lambda;

-                        //float epsilon = 0.001f;

-

-                        int numIter = 0;

-                        //first solution, using GJK

-

-

-                        Matrix identityTrans = Matrix.Identity;

-

-                        SphereShape raySphere = new SphereShape(0f);

-                        raySphere.Margin=0f;

-

-

-                        //result.drawCoordSystem(sphereTr);

-

-                        PointCollector pointCollector1 = new PointCollector();

-

-                        GjkPairDetector gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);

-                        GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();

-

-                        //we don't use margins during CCD

-                        gjk.setIgnoreMargin(true);

-

-                        input.TransformA = fromA;

-                        input.TransformB = fromB;

-

-                        DiscreteCollisionDetectorInterface.Result r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;

-                        gjk.GetClosestPoints(input, r, null);

-

-                        hasResult = pointCollector1.HasResult;

-                        c = pointCollector1.PointInWorld;

-

-                        if (hasResult)

-                        {

-                                float dist;

-                                dist = pointCollector1.Distance;

-                                n = pointCollector1.NormalOnBInWorld;

-

-                                //not close enough

-                                while (dist > radius)

-                                {

-                                        numIter++;

-                                        if (numIter > maxIter)

-                                                return false; //todo: report a failure

-

-                                        float dLambda = 0f;

-

-                                        //calculate safe moving fraction from distance / (linear+rotational velocity)

-

-                                        //float clippedDist  = GEN_min(angularConservativeRadius,dist);

-                                        //float clippedDist  = dist;

-

-                                        float projectedLinearVelocity = Vector3.Dot(linVelB - linVelA, n);

-

-                                        dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);

-

-                                        lambda = lambda + dLambda;

-

-                                        if (lambda > 1f) return false;

-                                        if (lambda < 0f) return false;

-

-                                        //todo: next check with relative epsilon

-                                        if (lambda <= lastLambda)

-                                                break;

-                                        lastLambda = lambda;

-

-

-                                        //interpolate to next lambda

-                                        Matrix interpolatedTransA = new Matrix(), interpolatedTransB = new Matrix(), relativeTrans;

-

-                                        TransformUtil.IntegrateTransform(fromA, linVelA, angVelA, lambda, ref interpolatedTransA);

-                                        TransformUtil.IntegrateTransform(fromB, linVelB, angVelB, lambda, ref interpolatedTransB);

-

-                                        relativeTrans = MathHelper.InverseTimes(interpolatedTransB, interpolatedTransA);

-

-                                        result.DebugDraw(lambda);

-

-                                        PointCollector pointCollector = new PointCollector();

-                                        gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);

-                                        input = new DiscreteCollisionDetectorInterface.ClosestPointInput();

-                                        input.TransformA = interpolatedTransA;

-                                        input.TransformB = interpolatedTransB;

-

-                                        // !!!!!!!!!!

-                                        r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;

-                                        gjk.GetClosestPoints(input, r, null);

-

-                                        if (pointCollector.HasResult)

-                                        {

-                                                if (pointCollector.Distance < 0f)

-                                                {

-                                                        //degenerate ?!

-                                                        result.Fraction = lastLambda;

-                                                        result.Normal = n;

-                                                        return true;

-                                                }

-                                                c = pointCollector.PointInWorld;

-

-                                                dist = pointCollector.Distance;

-                                        }

-                                        else

-                                        {

-                                                //??

-                                                return false;

-                                        }

-

-                                }

-

-                                result.Fraction = lambda;

-                                result.Normal = n;

-                                return true;

-                        }

-

-                        return false;

-                }

-        }

-}

+/*
+  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
+  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using MonoXnaCompactMaths;
+
+namespace XnaDevRu.BulletX
+{
+        /// <summary>
+        /// ContinuousConvexCollision implements angular and linear time of impact for convex objects.
+        /// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis).
+        /// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent.
+        /// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops
+        /// </summary>
+        public class ContinuousConvexCollision : IConvexCast
+        {
+                /// <summary>
+                /// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
+                /// You don't want your game ever to lock-up.
+                /// </summary>
+                private const int MaxIterations = 1000;
+
+                private ISimplexSolver _simplexSolver;
+                private IConvexPenetrationDepthSolver _penetrationDepthSolver;
+                private ConvexShape _convexA;
+                private ConvexShape _convexB;
+
+                public ContinuousConvexCollision(ConvexShape convexA, ConvexShape convexB,
+                        ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
+                {
+                        _simplexSolver = simplexSolver;
+                        _penetrationDepthSolver = penetrationDepthSolver;
+                        _convexA = convexA;
+                        _convexB = convexB;
+                }
+
+                public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
+                {
+                        _simplexSolver.Reset();
+
+                        // compute linear and angular velocity for this interval, to interpolate
+                        Vector3 linVelA = new Vector3(), angVelA = new Vector3(), linVelB = new Vector3(), angVelB = new Vector3();
+                        TransformUtil.CalculateVelocity(fromA, toA, 1f, ref linVelA, ref angVelA);
+                        TransformUtil.CalculateVelocity(fromB, toB, 1f, ref linVelB, ref angVelB);
+
+                        float boundingRadiusA = _convexA.GetAngularMotionDisc();
+                        float boundingRadiusB = _convexB.GetAngularMotionDisc();
+
+                        float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA +
+                                angVelB.Length() * boundingRadiusB;
+
+                        float radius = 0.001f;
+
+                        float lambda = 0f;
+                        Vector3 v = new Vector3(1f, 0f, 0f);
+
+                        int maxIter = MaxIterations;
+
+                        Vector3 n = new Vector3();
+                        bool hasResult = false;
+                        Vector3 c;
+
+                        float lastLambda = lambda;
+                        //float epsilon = 0.001f;
+
+                        int numIter = 0;
+                        //first solution, using GJK
+
+
+                        Matrix identityTrans = Matrix.Identity;
+
+                        SphereShape raySphere = new SphereShape(0f);
+                        raySphere.Margin=0f;
+
+
+                        //result.drawCoordSystem(sphereTr);
+
+                        PointCollector pointCollector1 = new PointCollector();
+
+                        GjkPairDetector gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
+                        GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
+
+                        //we don't use margins during CCD
+                        gjk.setIgnoreMargin(true);
+
+                        input.TransformA = fromA;
+                        input.TransformB = fromB;
+
+                        DiscreteCollisionDetectorInterface.Result r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
+                        gjk.GetClosestPoints(input, r, null);
+
+                        hasResult = pointCollector1.HasResult;
+                        c = pointCollector1.PointInWorld;
+
+                        if (hasResult)
+                        {
+                                float dist;
+                                dist = pointCollector1.Distance;
+                                n = pointCollector1.NormalOnBInWorld;
+
+                                //not close enough
+                                while (dist > radius)
+                                {
+                                        numIter++;
+                                        if (numIter > maxIter)
+                                                return false; //todo: report a failure
+
+                                        float dLambda = 0f;
+
+                                        //calculate safe moving fraction from distance / (linear+rotational velocity)
+
+                                        //float clippedDist  = GEN_min(angularConservativeRadius,dist);
+                                        //float clippedDist  = dist;
+
+                                        float projectedLinearVelocity = Vector3.Dot(linVelB - linVelA, n);
+
+                                        dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);
+
+                                        lambda = lambda + dLambda;
+
+                                        if (lambda > 1f) return false;
+                                        if (lambda < 0f) return false;
+
+                                        //todo: next check with relative epsilon
+                                        if (lambda <= lastLambda)
+                                                break;
+                                        lastLambda = lambda;
+
+
+                                        //interpolate to next lambda
+                                        Matrix interpolatedTransA = new Matrix(), interpolatedTransB = new Matrix(), relativeTrans;
+
+                                        TransformUtil.IntegrateTransform(fromA, linVelA, angVelA, lambda, ref interpolatedTransA);
+                                        TransformUtil.IntegrateTransform(fromB, linVelB, angVelB, lambda, ref interpolatedTransB);
+
+                                        relativeTrans = MathHelper.InverseTimes(interpolatedTransB, interpolatedTransA);
+
+                                        result.DebugDraw(lambda);
+
+                                        PointCollector pointCollector = new PointCollector();
+                                        gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
+                                        input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
+                                        input.TransformA = interpolatedTransA;
+                                        input.TransformB = interpolatedTransB;
+
+                                        // !!!!!!!!!!
+                                        r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
+                                        gjk.GetClosestPoints(input, r, null);
+
+                                        if (pointCollector.HasResult)
+                                        {
+                                                if (pointCollector.Distance < 0f)
+                                                {
+                                                        //degenerate ?!
+                                                        result.Fraction = lastLambda;
+                                                        result.Normal = n;
+                                                        return true;
+                                                }
+                                                c = pointCollector.PointInWorld;
+
+                                                dist = pointCollector.Distance;
+                                        }
+                                        else
+                                        {
+                                                //??
+                                                return false;
+                                        }
+
+                                }
+
+                                result.Fraction = lambda;
+                                result.Normal = n;
+                                return true;
+                        }
+
+                        return false;
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs
index 9e88a7e..93501ed 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs
@@ -1,73 +1,73 @@
-/*

-  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru

-  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

-

-  This software is provided 'as-is', without any express or implied

-  warranty.  In no event will the authors be held liable for any damages

-  arising from the use of this software.

-

-  Permission is granted to anyone to use this software for any purpose,

-  including commercial applications, and to alter it and redistribute it

-  freely, subject to the following restrictions:

-

-  1. The origin of this software must not be misrepresented; you must not

-     claim that you wrote the original software. If you use this software

-     in a product, an acknowledgment in the product documentation would be

-     appreciated but is not required.

-  2. Altered source versions must be plainly marked as such, and must not be

-     misrepresented as being the original software.

-  3. This notice may not be removed or altered from any source distribution.

-*/

-

-using System;

-using System.Collections.Generic;

-using System.Text;

-using MonoXnaCompactMaths;

-

-namespace XnaDevRu.BulletX

-{

-

-    /// <summary>

-    /// CastResult stores the closest result

-    /// alternatively, add a callback method to decide about closest/all results

-    /// </summary>

-        public class CastResult

-        {

-                private Vector3 _normal;

-                private float _fraction;

-                private Matrix _hitTransformA;

-                private Matrix _hitTransformB;

-                private IDebugDraw _debugDrawer;

-

-                public CastResult()

-                {

-                        _fraction = 1e30f;

-                }

-

-                public Vector3 Normal { get { return _normal; } set { _normal = value; } }

-                public float Fraction { get { return _fraction; } set { _fraction = value; } }

-                public Matrix HitTransformA { get { return _hitTransformA; } set { _hitTransformA = value; } }

-                public Matrix HitTransformB { get { return _hitTransformB; } set { _hitTransformB = value; } }

-                public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } }

-

-                public virtual void DebugDraw(float fraction) { }

-                public virtual void DrawCoordSystem(Matrix trans) { }

-        }

-

-        /// <summary>

-    /// ConvexCast is an interface for Casting

-        /// </summary>

-        public interface IConvexCast

-        {

-                /// <summary>

-        /// cast a convex against another convex object

-                /// </summary>

-                /// <param name="fromA"></param>

-                /// <param name="toA"></param>

-                /// <param name="fromB"></param>

-                /// <param name="toB"></param>

-                /// <param name="result"></param>

-                /// <returns></returns>

-                bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result);

-        }

-}

+/*
+  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
+  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using MonoXnaCompactMaths;
+
+namespace XnaDevRu.BulletX
+{
+
+    /// <summary>
+    /// CastResult stores the closest result
+    /// alternatively, add a callback method to decide about closest/all results
+    /// </summary>
+        public class CastResult
+        {
+                private Vector3 _normal;
+                private float _fraction;
+                private Matrix _hitTransformA;
+                private Matrix _hitTransformB;
+                private IDebugDraw _debugDrawer;
+
+                public CastResult()
+                {
+                        _fraction = 1e30f;
+                }
+
+                public Vector3 Normal { get { return _normal; } set { _normal = value; } }
+                public float Fraction { get { return _fraction; } set { _fraction = value; } }
+                public Matrix HitTransformA { get { return _hitTransformA; } set { _hitTransformA = value; } }
+                public Matrix HitTransformB { get { return _hitTransformB; } set { _hitTransformB = value; } }
+                public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } }
+
+                public virtual void DebugDraw(float fraction) { }
+                public virtual void DrawCoordSystem(Matrix trans) { }
+        }
+
+        /// <summary>
+    /// ConvexCast is an interface for Casting
+        /// </summary>
+        public interface IConvexCast
+        {
+                /// <summary>
+        /// cast a convex against another convex object
+                /// </summary>
+                /// <param name="fromA"></param>
+                /// <param name="toA"></param>
+                /// <param name="fromB"></param>
+                /// <param name="toB"></param>
+                /// <param name="result"></param>
+                /// <returns></returns>
+                bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result);
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs
index fae2557..5b07d9b 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs
@@ -1,117 +1,117 @@
-/*

-  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru

-  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

-

-  This software is provided 'as-is', without any express or implied

-  warranty.  In no event will the authors be held liable for any damages

-  arising from the use of this software.

-

-  Permission is granted to anyone to use this software for any purpose,

-  including commercial applications, and to alter it and redistribute it

-  freely, subject to the following restrictions:

-

-  1. The origin of this software must not be misrepresented; you must not

-     claim that you wrote the original software. If you use this software

-     in a product, an acknowledgment in the product documentation would be

-     appreciated but is not required.

-  2. Altered source versions must be plainly marked as such, and must not be

-     misrepresented as being the original software.

-  3. This notice may not be removed or altered from any source distribution.

-*/

-

-using System;

-using System.Collections.Generic;

-using System.Text;

-using MonoXnaCompactMaths;

-

-namespace XnaDevRu.BulletX

-{

-        public abstract class DiscreteCollisionDetectorInterface

-        {

-                public abstract class Result

-                {

-                        public abstract void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB);

-                        public abstract void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth);

-                }

-

-                public class ClosestPointInput

-                {

-                        private float _maximumDistanceSquared;

-                        private Matrix _transformA, _transformB;

-

-                        #region Properties

-                        public Matrix TransformB

-                        {

-                                get { return _transformB; }

-                                set { _transformB = value; }

-                        }

-

-                        public Matrix TransformA

-                        {

-                                get { return _transformA; }

-                                set { _transformA = value; }

-                        }

-

-                        public float MaximumDistanceSquared

-                        {

-                                get { return _maximumDistanceSquared; }

-                                set { _maximumDistanceSquared = value; }

-                        }

-                        #endregion

-

-                        public ClosestPointInput()

-                        {

-                                _maximumDistanceSquared = 1e30f;

-                        }

-                }

-

-                public abstract void GetClosestPoints(ClosestPointInput input, Result output, IDebugDraw debugDraw);

-        }

-

-        public class StorageResult : DiscreteCollisionDetectorInterface.Result

-        {

-                private Vector3 _closestPointInB;

-                private Vector3 _normalOnSurfaceB;

-                private float _distance; //negative means penetration !

-

-                #region Properties

-

-                public float Distance

-                {

-                        get { return _distance; }

-                        set { _distance = value; }

-                }

-                public Vector3 NormalOnSurfaceB

-                {

-                        get { return _normalOnSurfaceB; }

-                        set { _normalOnSurfaceB = value; }

-                }

-                public Vector3 ClosestPointInB

-                {

-                        get { return _closestPointInB; }

-                        set { _closestPointInB = value; }

-                }

-

-                #endregion

-

-                public StorageResult()

-                {

-                        _distance = 1e30f;

-                }

-

-                public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth)

-                {

-                        if (depth < _distance)

-                        {

-                                _normalOnSurfaceB = normalOnBInWorld;

-                                _closestPointInB = pointInWorld;

-                                _distance = depth;

-                        }

-                }

-

-                public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1)

-                {

-                        throw new Exception("The method or operation is not implemented.");

-                }

-        }

-}

+/*
+  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
+  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using MonoXnaCompactMaths;
+
+namespace XnaDevRu.BulletX
+{
+        public abstract class DiscreteCollisionDetectorInterface
+        {
+                public abstract class Result
+                {
+                        public abstract void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB);
+                        public abstract void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth);
+                }
+
+                public class ClosestPointInput
+                {
+                        private float _maximumDistanceSquared;
+                        private Matrix _transformA, _transformB;
+
+                        #region Properties
+                        public Matrix TransformB
+                        {
+                                get { return _transformB; }
+                                set { _transformB = value; }
+                        }
+
+                        public Matrix TransformA
+                        {
+                                get { return _transformA; }
+                                set { _transformA = value; }
+                        }
+
+                        public float MaximumDistanceSquared
+                        {
+                                get { return _maximumDistanceSquared; }
+                                set { _maximumDistanceSquared = value; }
+                        }
+                        #endregion
+
+                        public ClosestPointInput()
+                        {
+                                _maximumDistanceSquared = 1e30f;
+                        }
+                }
+
+                public abstract void GetClosestPoints(ClosestPointInput input, Result output, IDebugDraw debugDraw);
+        }
+
+        public class StorageResult : DiscreteCollisionDetectorInterface.Result
+        {
+                private Vector3 _closestPointInB;
+                private Vector3 _normalOnSurfaceB;
+                private float _distance; //negative means penetration !
+
+                #region Properties
+
+                public float Distance
+                {
+                        get { return _distance; }
+                        set { _distance = value; }
+                }
+                public Vector3 NormalOnSurfaceB
+                {
+                        get { return _normalOnSurfaceB; }
+                        set { _normalOnSurfaceB = value; }
+                }
+                public Vector3 ClosestPointInB
+                {
+                        get { return _closestPointInB; }
+                        set { _closestPointInB = value; }
+                }
+
+                #endregion
+
+                public StorageResult()
+                {
+                        _distance = 1e30f;
+                }
+
+                public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth)
+                {
+                        if (depth < _distance)
+                        {
+                                _normalOnSurfaceB = normalOnBInWorld;
+                                _closestPointInB = pointInWorld;
+                                _distance = depth;
+                        }
+                }
+
+                public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1)
+                {
+                        throw new Exception("The method or operation is not implemented.");
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs
index 4ee9599..ea7d335 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs
@@ -1,176 +1,176 @@
-/*

-  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru

-  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

-

-  This software is provided 'as-is', without any express or implied

-  warranty.  In no event will the authors be held liable for any damages

-  arising from the use of this software.

-

-  Permission is granted to anyone to use this software for any purpose,

-  including commercial applications, and to alter it and redistribute it

-  freely, subject to the following restrictions:

-

-  1. The origin of this software must not be misrepresented; you must not

-     claim that you wrote the original software. If you use this software

-     in a product, an acknowledgment in the product documentation would be

-     appreciated but is not required.

-  2. Altered source versions must be plainly marked as such, and must not be

-     misrepresented as being the original software.

-  3. This notice may not be removed or altered from any source distribution.

-*/

-

-using System;

-using System.Collections.Generic;

-using System.Text;

-using MonoXnaCompactMaths;

-

-namespace XnaDevRu.BulletX

-{

-    /// <summary>

-    /// GjkConvexCast performs a raycast on a convex object using support mapping.

-    /// </summary>

-        public class GjkConvexCast : IConvexCast

-        {

-                private VoronoiSimplexSolver _simplexSolver;

-                private ConvexShape _convexA, _convexB;

-

-                public GjkConvexCast(ConvexShape convexShapeA, ConvexShape convexShapeB, VoronoiSimplexSolver solver)

-                {

-                        _simplexSolver = solver;

-

-                        _convexA = convexShapeA;

-                        _convexB = convexShapeB;

-                }

-

-                #region IConvexCast Members

-

-        /// <summary>

-        /// cast a convex against another convex object

-        /// </summary>

-        /// <param name="fromA"></param>

-        /// <param name="toA"></param>

-        /// <param name="fromB"></param>

-        /// <param name="toB"></param>

-        /// <param name="result"></param>

-        /// <returns></returns>

-                public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)

-                {

-                        MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB);

-

-            Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;

-            Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB;

-

-                        Matrix transformA = fromA;

-                        Matrix transformB = fromB;

-

-                        transformA.Translation = new Vector3(0, 0, 0);

-                        transformB.Translation = new Vector3(0, 0, 0);

-

-                        combined.TransformA = transformA;

-                        combined.TransformB = transformB;

-

-                        float radius = 0.01f;

-                        float lambda = 0;

-

-                        Vector3 s = rayFromLocalA.Translation;

-                        Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation;

-                        Vector3 x = s;

-                        Vector3 n = new Vector3();

-                        Vector3 c = new Vector3();

-

-                        bool hasResult = false;

-                        float lastLambda = lambda;

-

-                        IConvexPenetrationDepthSolver penSolver = null;

-                        Matrix identityTransform = Matrix.Identity;

-

-                        SphereShape raySphere = new SphereShape(0.0f);

-                        raySphere.Margin=0.0f;

-

-                        Matrix sphereTransform = Matrix.Identity;

-                        sphereTransform.Translation = rayFromLocalA.Translation;

-

-                        result.DrawCoordSystem(sphereTransform);

-

-                        {

-                                PointCollector pointCollector = new PointCollector();

-                                GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);

-

-                                GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();

-                                input.TransformA = sphereTransform;

-                                input.TransformB = identityTransform;

-

-                                gjk.GetClosestPoints(input, pointCollector, null);

-

-                                hasResult = pointCollector.HasResult;

-

-                                c = pointCollector.PointInWorld;

-                                n = pointCollector.NormalOnBInWorld;

-                        }

-

-                        if (hasResult)

-                        {

-                                float dist = (c - x).Length();

-

-                                if (dist < radius)

-                                {

-                                        lastLambda = 1.0f;

-                                }

-

-                                while (dist > radius)

-                                {

-                                        n = x - c;

-                                        float dot = Vector3.Dot(n, r);

-

-                                        if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon)) return false;

-

-                                        lambda = lambda - Vector3.Distance(n, n) / dot;

-                                        if (lambda <= lastLambda) break;

-

-                                        lastLambda = lambda;

-

-                                        x = s + lambda * r;

-

-                                        sphereTransform.Translation = x;

-                                        result.DrawCoordSystem(sphereTransform);

-                                        PointCollector pointCollector = new PointCollector();

-

-                                        GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);

-                                        GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();

-                                        input.TransformA = sphereTransform;

-                                        input.TransformB = identityTransform;

-

-                                        gjk.GetClosestPoints(input, pointCollector, null);

-

-                                        if (pointCollector.HasResult)

-                                        {

-                                                if (pointCollector.Distance < 0.0f)

-                                                {

-                                                        result.Fraction = lastLambda;

-                                                        result.Normal = n;

-                                                        return true;

-                                                }

-

-                                                c = pointCollector.PointInWorld;

-                                                dist = (c - x).Length();

-                                        }

-                                        else

-                                        {

-                                                return false;

-                                        }

-                                }

-

-                                if (lastLambda < 1.0f)

-                                {

-                                        result.Fraction = lastLambda;

-                                        result.Normal = n;

-                                        return true;

-                                }

-                        }

-

-                        return false;

-                }

-

-                #endregion

-        }

-}

+/*
+  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
+  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using MonoXnaCompactMaths;
+
+namespace XnaDevRu.BulletX
+{
+    /// <summary>
+    /// GjkConvexCast performs a raycast on a convex object using support mapping.
+    /// </summary>
+        public class GjkConvexCast : IConvexCast
+        {
+                private VoronoiSimplexSolver _simplexSolver;
+                private ConvexShape _convexA, _convexB;
+
+                public GjkConvexCast(ConvexShape convexShapeA, ConvexShape convexShapeB, VoronoiSimplexSolver solver)
+                {
+                        _simplexSolver = solver;
+
+                        _convexA = convexShapeA;
+                        _convexB = convexShapeB;
+                }
+
+                #region IConvexCast Members
+
+        /// <summary>
+        /// cast a convex against another convex object
+        /// </summary>
+        /// <param name="fromA"></param>
+        /// <param name="toA"></param>
+        /// <param name="fromB"></param>
+        /// <param name="toB"></param>
+        /// <param name="result"></param>
+        /// <returns></returns>
+                public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
+                {
+                        MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB);
+
+            Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;
+            Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB;
+
+                        Matrix transformA = fromA;
+                        Matrix transformB = fromB;
+
+                        transformA.Translation = new Vector3(0, 0, 0);
+                        transformB.Translation = new Vector3(0, 0, 0);
+
+                        combined.TransformA = transformA;
+                        combined.TransformB = transformB;
+
+                        float radius = 0.01f;
+                        float lambda = 0;
+
+                        Vector3 s = rayFromLocalA.Translation;
+                        Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation;
+                        Vector3 x = s;
+                        Vector3 n = new Vector3();
+                        Vector3 c = new Vector3();
+
+                        bool hasResult = false;
+                        float lastLambda = lambda;
+
+                        IConvexPenetrationDepthSolver penSolver = null;
+                        Matrix identityTransform = Matrix.Identity;
+
+                        SphereShape raySphere = new SphereShape(0.0f);
+                        raySphere.Margin=0.0f;
+
+                        Matrix sphereTransform = Matrix.Identity;
+                        sphereTransform.Translation = rayFromLocalA.Translation;
+
+                        result.DrawCoordSystem(sphereTransform);
+
+                        {
+                                PointCollector pointCollector = new PointCollector();
+                                GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
+
+                                GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
+                                input.TransformA = sphereTransform;
+                                input.TransformB = identityTransform;
+
+                                gjk.GetClosestPoints(input, pointCollector, null);
+
+                                hasResult = pointCollector.HasResult;
+
+                                c = pointCollector.PointInWorld;
+                                n = pointCollector.NormalOnBInWorld;
+                        }
+
+                        if (hasResult)
+                        {
+                                float dist = (c - x).Length();
+
+                                if (dist < radius)
+                                {
+                                        lastLambda = 1.0f;
+                                }
+
+                                while (dist > radius)
+                                {
+                                        n = x - c;
+                                        float dot = Vector3.Dot(n, r);
+
+                                        if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon)) return false;
+
+                                        lambda = lambda - Vector3.Distance(n, n) / dot;
+                                        if (lambda <= lastLambda) break;
+
+                                        lastLambda = lambda;
+
+                                        x = s + lambda * r;
+
+                                        sphereTransform.Translation = x;
+                                        result.DrawCoordSystem(sphereTransform);
+                                        PointCollector pointCollector = new PointCollector();
+
+                                        GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
+                                        GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
+                                        input.TransformA = sphereTransform;
+                                        input.TransformB = identityTransform;
+
+                                        gjk.GetClosestPoints(input, pointCollector, null);
+
+                                        if (pointCollector.HasResult)
+                                        {
+                                                if (pointCollector.Distance < 0.0f)
+                                                {
+                                                        result.Fraction = lastLambda;
+                                                        result.Normal = n;
+                                                        return true;
+                                                }
+
+                                                c = pointCollector.PointInWorld;
+                                                dist = (c - x).Length();
+                                        }
+                                        else
+                                        {
+                                                return false;
+                                        }
+                                }
+
+                                if (lastLambda < 1.0f)
+                                {
+                                        result.Fraction = lastLambda;
+                                        result.Normal = n;
+                                        return true;
+                                }
+                        }
+
+                        return false;
+                }
+
+                #endregion
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs
index 50ae7ab..a3064db 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs
@@ -1,633 +1,633 @@
-/*

-  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru

-  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

-

-  This software is provided 'as-is', without any express or implied

-  warranty.  In no event will the authors be held liable for any damages

-  arising from the use of this software.

-

-  Permission is granted to anyone to use this software for any purpose,

-  including commercial applications, and to alter it and redistribute it

-  freely, subject to the following restrictions:

-

-  1. The origin of this software must not be misrepresented; you must not

-     claim that you wrote the original software. If you use this software

-     in a product, an acknowledgment in the product documentation would be

-     appreciated but is not required.

-  2. Altered source versions must be plainly marked as such, and must not be

-     misrepresented as being the original software.

-  3. This notice may not be removed or altered from any source distribution.

-*/

-

-using System;

-using System.Collections.Generic;

-using System.Text;

-using MonoXnaCompactMaths;

-

-namespace XnaDevRu.BulletX

-{

-        /// <summary>

-        /// GJK-EPA collision solver by Nathanael Presson

-        /// Nov.2006

-        /// </summary>

-        public class GjkEpa

-        {

-                //private static readonly int _precision = 1 /* U(sizeof(F) == 4)*/;

-

-                private static readonly float _infinity = MathHelper.Infinity;

-                //private static readonly float _pi = (float)Math.PI;

-                private static readonly float _twoPi = (float)(Math.PI * 2);

-

-                private static readonly int _gjkMaxIterations = 128;

-                private static readonly int _gjkHashSize = 1 << 6;

-                private static readonly int _gjkHashMask = _gjkHashSize - 1;

-                private static readonly float _gjkInSimplexEpsilon = 0.0001f;

-                private static readonly float _gjkSquareInSimplexEpsilon = _gjkInSimplexEpsilon * _gjkInSimplexEpsilon;

-

-                private static readonly int _epaMaxIterations = 256;

-                private static readonly float _epaInFaceEpsilon = 0.01f;

-                private static readonly float _epaAccuracy = 0.001f;

-

-                public static float EpaAccuracy { get { return _epaAccuracy; } }

-

-                private static float Abs(float v) { return (v < 0 ? -v : v); }

-                private static float Sign(float v) { return (v < 0 ? -1 : 1); }

-

-                static void Swap<T>(ref T a, ref T b)

-                {

-                        T t = a;

-                        a = b;

-                        b = t;

-                }

-

-                public class Gjk

-                {

-                        public class MinkowskiVertice

-                        {

-                                private Vector3 _vertice;       /* Minkowski vertice    */

-                                private Vector3 _ray;           /* Ray                                  */

-

-                                public Vector3 Vertice { get { return _vertice; } set { _vertice = value; } }

-                                public Vector3 Ray { get { return _ray; } set { _ray = value; } }

-                        }

-                        public class He

-                        {

-                                private Vector3 _ray;

-                                private He _next;

-

-                                public He Next { get { return _next; } set { _next = value; } }

-                                public Vector3 Ray { get { return _ray; } set { _ray = value; } }

-                        }

-

-                        private He[] _table = new He[_gjkHashSize];

-                        private Matrix[] _wrotations = new Matrix[2];

-                        private Vector3[] _positions = new Vector3[2];

-                        private ConvexShape[] _shapes = new ConvexShape[2];

-                        private MinkowskiVertice[] _simplex = new MinkowskiVertice[5];

-                        private Vector3 _ray;

-                        private int _order;

-                        private int _iterations;

-                        private float _margin;

-                        private bool _failed;

-

-                        public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA,

-                                                Matrix wrotationB, Vector3 positionB, ConvexShape shapeB)

-                                : this(wrotationA, positionA, shapeA, wrotationB, positionB, shapeB, 0) { }

-

-                        public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA,

-                                                Matrix wrotationB, Vector3 positionB, ConvexShape shapeB,

-                                                float pmargin)

-                        {

-                for (int i = 0; i < _simplex.Length; i++)

-                    _simplex[i] = new MinkowskiVertice();

-

-                for (int i = 0; i < _wrotations.Length; i++)

-                    _wrotations[i] = new Matrix();

-

-                for (int i = 0; i < _positions.Length; i++)

-                    _positions[i] = new Vector3();

-

-                _wrotations[0] = wrotationA; _positions[0] = positionA; 

-                                _shapes[0] = shapeA;

-                _wrotations[0].Translation = Vector3.Zero;

-                                _wrotations[1] = wrotationB; _positions[1] = positionB; 

-                                _shapes[1] = shapeB;

-                _wrotations[1].Translation = Vector3.Zero;

-                                //sablock = sa->BeginBlock();

-                                _margin = pmargin;

-                                _failed = false;

-                        }

-

-                        public bool Failed { get { return _failed; } }

-                        public int Iterations { get { return _iterations; } }

-                        public int Order { get { return _order; } }

-                        public MinkowskiVertice[] Simplex { get { return _simplex; } }

-

-                        public int Hash(Vector3 v)

-                        {

-                                int h = ((int)(v.X * 15461) ^ (int)(v.Y * 83003) ^ (int)(v.Z * 15473));

-                                return (h * 169639) & _gjkHashMask;

-                        }

-

-                        public bool FetchSupport()

-                        {

-                                int h = Hash(_ray);

-                                He e = _table[h];

-                                while (e != null)

-                                {

-                                        if (e.Ray == _ray)

-                                        {

-                                                --_order;

-                                                return (false);

-                                        }

-                                        else e = e.Next;

-                                }

-                                e = new He(); 

-                                e.Ray = _ray; 

-                                e.Next = _table[h]; 

-                                _table[h] = e;

-                                Support(_ray, ref _simplex[++_order]);

-                                return (Vector3.Dot(_ray, _simplex[_order].Vertice) > 0);

-                        }

-

-                        public Vector3 LocalSupport(Vector3 d, int i)

-                        {

-                Matrix m = _wrotations[i];

-                m.Translation = Vector3.Zero;

-                Vector3 vtx = Vector3.TransformNormal(d, m);

-                Vector3 result = MathHelper.MatrixToVector(_wrotations[i], _shapes[i].LocalGetSupportingVertex(vtx));

-                return (result + _positions[i]);

-                        }

-

-                        public void Support(Vector3 d, ref MinkowskiVertice v)

-                        {

-                                v.Ray = d;

-                                v.Vertice = LocalSupport(d, 0) - LocalSupport(-d, 1) + d * _margin;

-                        }

-

-                        public bool SolveSimplex2(Vector3 ao, Vector3 ab)

-                        {

-                                if (Vector3.Dot(ab, ao) >= 0)

-                                {

-                                        Vector3 cabo = Vector3.Cross(ab, ao);

-                                        if (cabo.LengthSquared() > _gjkSquareInSimplexEpsilon)

-                                        { _ray = Vector3.Cross(cabo, ab); }

-                                        else

-                                        { return true; }

-                                }

-                                else

-                                {

-                    _order = 0;

-                                        _simplex[0].Ray = _simplex[1].Ray;

-                                        _simplex[0].Vertice = _simplex[1].Vertice;

-

-                    _ray = ao; 

-                }

-                                return false;

-                        }

-

-                        public bool SolveSimplex3(Vector3 ao, Vector3 ab, Vector3 ac)

-                        {

-                                return (SolveSimplex3a(ao, ab, ac, Vector3.Cross(ab, ac)));

-                        }

-

-                        public bool SolveSimplex3a(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 cabc)

-                        {

-                                if ((Vector3.Dot(Vector3.Cross(cabc, ab), ao)) < -_gjkInSimplexEpsilon)

-                                {

-                    _order = 1;

-                                        _simplex[0].Vertice = _simplex[1].Vertice;

-                                        _simplex[0].Ray = _simplex[1].Ray;

-

-                                        _simplex[1].Vertice = _simplex[2].Vertice;

-                                        _simplex[1].Ray = _simplex[2].Ray;

-

-                    return (SolveSimplex2(ao, ab));

-                }

-                                else if (Vector3.Dot(Vector3.Cross(cabc, ac), ao) > +_gjkInSimplexEpsilon)

-                                {

-                    _order = 1;

-                                        _simplex[1].Vertice = _simplex[2].Vertice;

-                                        _simplex[1].Ray = _simplex[2].Ray;

-

-                    return (SolveSimplex2(ao, ac));

-                }

-                                else

-                                {

-                                        float d = Vector3.Dot(cabc, ao);

-                                        if (Abs(d) > _gjkInSimplexEpsilon)

-                                        {

-                                                if (d > 0)

-                                                { _ray = cabc; }

-                                                else

-                                                { _ray = -cabc; Swap<MinkowskiVertice>(ref _simplex[0], ref _simplex[1]); }

-                                                return (false);

-                                        }

-                                        else return (true);

-                                }

-                        }

-

-                        public bool SolveSimplex4(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 ad)

-                        {

-                                Vector3 crs;

-                                if (Vector3.Dot((crs = Vector3.Cross(ab, ac)), ao) > _gjkInSimplexEpsilon)

-                                {

-                                        _order = 2;

-                                        _simplex[0].Vertice = _simplex[1].Vertice;

-                                        _simplex[0].Ray = _simplex[1].Ray;

-

-                                        _simplex[1].Vertice = _simplex[2].Vertice;

-                                        _simplex[1].Ray = _simplex[2].Ray;

-

-                                        _simplex[2].Vertice = _simplex[3].Vertice;

-                                        _simplex[2].Ray = _simplex[3].Ray;

-                    

-                    return (SolveSimplex3a(ao, ab, ac, crs));

-                                }

-                                else if (Vector3.Dot((crs = Vector3.Cross(ac, ad)), ao) > _gjkInSimplexEpsilon)

-                                { 

-                    _order = 2;

-                                        _simplex[2].Vertice = _simplex[3].Vertice;

-                                        _simplex[2].Ray = _simplex[3].Ray;

-                    

-                    return (SolveSimplex3a(ao, ac, ad, crs)); 

-                }

-                                else if (Vector3.Dot((crs = Vector3.Cross(ad, ab)), ao) > _gjkInSimplexEpsilon)

-                                {

-                                        _order = 2;

-

-                                        _simplex[1].Vertice = _simplex[0].Vertice;

-                                        _simplex[1].Ray = _simplex[0].Ray;

-

-                                        _simplex[0].Vertice = _simplex[2].Vertice;

-                                        _simplex[0].Ray = _simplex[2].Ray;

-

-                                        _simplex[2].Vertice = _simplex[3].Vertice;

-                                        _simplex[2].Ray = _simplex[3].Ray; 

-                    

-                    return (SolveSimplex3a(ao, ad, ab, crs));

-                                }

-                                else return (true);

-                        }

-

-                        public bool SearchOrigin()

-                        {

-                                return SearchOrigin(new Vector3(1, 0, 0));

-                        }

-

-                        public bool SearchOrigin(Vector3 initray)

-                        {

-                                _iterations = 0;

-                                unchecked

-                                {

-                                        _order = (int)(-1);

-                                }

-                                _failed = false;

-                                _ray = Vector3.Normalize(initray);

-

-                                //ClearMemory(table, sizeof(void*) * GJK_hashsize);

-                for (int i = 0; i < _table.Length; i++)

-                    _table[i] = null;

-                                FetchSupport();

-                                _ray = -_simplex[0].Vertice;

-                                for (; _iterations < _gjkMaxIterations; ++_iterations)

-                                {

-                                        float rl = _ray.Length();

-                                        _ray /= rl > 0 ? rl : 1;

-                                        if (FetchSupport())

-                                        {

-                                                bool found = (false);

-                                                switch (_order)

-                                                {

-                                                        case 1: found = SolveSimplex2(-_simplex[1].Vertice, _simplex[0].Vertice - _simplex[1].Vertice); break;

-                                                        case 2: found = SolveSimplex3(-_simplex[2].Vertice, _simplex[1].Vertice - _simplex[2].Vertice, _simplex[0].Vertice - _simplex[2].Vertice); break;

-                                                        case 3: found = SolveSimplex4(-_simplex[3].Vertice, _simplex[2].Vertice - _simplex[3].Vertice, _simplex[1].Vertice - _simplex[3].Vertice, _simplex[0].Vertice - _simplex[3].Vertice); break;

-                                                }

-                                                if (found) return (true);

-                                        }

-                                        else return (false);

-                                }

-                                _failed = true;

-                                return (false);

-                        }

-

-                        public bool EncloseOrigin()

-                        {

-                                switch (_order)

-                                {

-                                        /* Point                */

-                                        case 0: break;

-                                        /* Line                 */

-                                        case 1:

-                                                Vector3 ab = _simplex[1].Vertice - _simplex[0].Vertice;

-                                                Vector3[] b ={  Vector3.Cross(ab, new Vector3(1, 0, 0)),

-                                                                                        Vector3.Cross(ab, new Vector3(0, 1, 0)),

-                                                                                        Vector3.Cross(ab, new Vector3(0, 0, 1)) };

-                                                float[] m ={ b[0].LengthSquared(), b[1].LengthSquared(), b[2].LengthSquared() };

-                                                Matrix r = Matrix.CreateFromQuaternion(new Quaternion(Vector3.Normalize(ab), _twoPi / 3));

-                                                Vector3 w = b[m[0] > m[1] ? m[0] > m[2] ? 0 : 2 : m[1] > m[2] ? 1 : 2];

-                                                Support(Vector3.Normalize(w), ref _simplex[4]); w = Vector3.TransformNormal(w, r);

-                        Support(Vector3.Normalize(w), ref _simplex[2]); w = Vector3.TransformNormal(w, r);

-                        Support(Vector3.Normalize(w), ref _simplex[3]); w = Vector3.TransformNormal(w, r);

-                                                _order = 4;

-                                                return true;

-                                        /* Triangle             */

-                                        case 2:

-                                                Vector3 n = Vector3.Normalize(Vector3.Cross(_simplex[1].Vertice - _simplex[0].Vertice, _simplex[2].Vertice - _simplex[0].Vertice));

-                                                Support(n, ref _simplex[3]);

-                                                Support(-n, ref _simplex[4]);

-                                                _order = 4;

-                                                return true;

-                                        /* Tetrahedron  */

-                                        case 3: return (true);

-                                        /* Hexahedron   */

-                                        case 4: return (true);

-                                }

-                                return (false);

-                        }

-                }

-

-                public class Epa

-                {

-                        public class Face

-                        {

-                                public Gjk.MinkowskiVertice[] _vertices = new Gjk.MinkowskiVertice[3];

-                                public Face[] _faces = new Face[3];

-                                public int[] _e = new int[3];

-                                public Vector3 _n;

-                                public float _d;

-                                public int _mark;

-                                public Face _prev;

-                                public Face _next;

-                        }

-

-                        private Gjk _gjk;

-                        private Face _root;

-                        private int _nfaces;

-                        private int _iterations;

-                        private Vector3[,] _features = new Vector3[2, 3];

-                        private Vector3[] _nearest = new Vector3[2];

-                        private Vector3 _normal;

-                        private float _depth;

-                        private bool _failed;

-

-                        public Epa(Gjk gjk)

-                        {

-                                this._gjk = gjk;

-                        }

-

-                        public bool Failed { get { return _failed; } }

-                        public int Iterations { get { return _iterations; } }

-                        public Vector3 Normal { get { return _normal; } }

-                        public Vector3[] Nearest { get { return _nearest; } }

-

-                        public Vector3 GetCoordinates(Face face)

-                        {

-                                Vector3 o = face._n * -face._d;

-                                float[] a ={    Vector3.Cross(face._vertices[0].Vertice - o, face._vertices[1].Vertice - o).Length(),

-                                                                Vector3.Cross(face._vertices[1].Vertice - o, face._vertices[2].Vertice - o).Length(),

-                                                                Vector3.Cross(face._vertices[2].Vertice - o, face._vertices[0].Vertice - o).Length()};

-                                float sm = a[0] + a[1] + a[2];

-                                return (new Vector3(a[1], a[2], a[0]) / (sm > 0 ? sm : 1));

-                        }

-

-                        public Face FindBest()

-                        {

-                                Face bf = null;

-                                if (_root != null)

-                                {

-                                        Face cf = _root;

-                                        float bd = _infinity;

-                                        do

-                                        {

-                                                if (cf._d < bd) { bd = cf._d; bf = cf; }

-                                        } while (null != (cf = cf._next));

-                                }

-                                return bf;

-                        }

-

-                        public bool Set(ref Face f, Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c)

-                        {

-                                Vector3 nrm = Vector3.Cross(b.Vertice - a.Vertice, c.Vertice - a.Vertice);

-                                float len = nrm.Length();

-                                bool valid = (Vector3.Dot(Vector3.Cross(a.Vertice, b.Vertice), nrm) >= -_epaInFaceEpsilon &&

-                                                                 Vector3.Dot(Vector3.Cross(b.Vertice, c.Vertice), nrm) >= -_epaInFaceEpsilon &&

-                                                                 Vector3.Dot(Vector3.Cross(c.Vertice, a.Vertice), nrm) >= -_epaInFaceEpsilon);

-                                f._vertices[0] = a;

-                                f._vertices[1] = b;

-                                f._vertices[2] = c;

-                                f._mark = 0;

-                                f._n = nrm / (len > 0 ? len : _infinity);

-                                f._d = Max(0, -Vector3.Dot(f._n, a.Vertice));

-                                return valid;

-                        }

-

-                        public Face NewFace(Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c)

-                        {

-                                Face pf = new Face();

-                                if (Set(ref pf, a, b, c))

-                                {

-                                        if (_root != null) _root._prev = pf;

-                                        pf._prev = null;

-                                        pf._next = _root;

-                                        _root = pf;

-                                        ++_nfaces;

-                                }

-                                else

-                                {

-                                        pf._prev = pf._next = null;

-                                }

-                                return (pf);

-                        }

-

-                        public void Detach(ref Face face)

-                        {

-                                if (face._prev != null || face._next != null)

-                                {

-                                        --_nfaces;

-                                        if (face == _root)

-                                        {

-                                                _root = face._next;

-                                                _root._prev = null;

-                                        }

-                                        else

-                                        {

-                                                if (face._next == null)

-                                                {

-                                                        face._prev._next = null;

-                                                }

-                                                else

-                                                {

-                                                        face._prev._next = face._next;

-                                                        face._next._prev = face._prev;

-                                                }

-                                        }

-                                        face._prev = face._next = null;

-                                }

-                        }

-

-                        public void Link(ref Face f0, int e0, ref Face f1, int e1)

-                        {

-                                f0._faces[e0] = f1; f1._e[e1] = e0;

-                                f1._faces[e1] = f0; f0._e[e0] = e1;

-                        }

-

-                        public Gjk.MinkowskiVertice Support(Vector3 w)

-                        {

-                                Gjk.MinkowskiVertice v = new Gjk.MinkowskiVertice();

-                                _gjk.Support(w, ref v);

-                                return v;

-                        }

-

-                        private static int[] mod3 ={ 0, 1, 2, 0, 1 };

-

-                        public int BuildHorizon(int markid, Gjk.MinkowskiVertice w, ref Face f, int e, ref Face cf, ref Face ff)

-                        {

-                                int ne = (0);

-                                if (f._mark != markid)

-                                {

-                                        int e1 = (mod3[e + 1]);

-                                        if ((Vector3.Dot(f._n, w.Vertice) + f._d) > 0)

-                                        {

-                                                Face nf = NewFace(f._vertices[e1], f._vertices[e], w);

-                                                Link(ref nf, 0, ref f, e);

-                                                if (cf != null) Link(ref cf, 1, ref nf, 2); else ff = nf;

-                                                cf = nf; ne = 1;

-                                        }

-                                        else

-                                        {

-                                                int e2 = (mod3[e + 2]);

-                                                Detach(ref f);

-                                                f._mark = markid;

-                                                ne += BuildHorizon(markid, w, ref f._faces[e1], f._e[e1], ref cf, ref ff);

-                                                ne += BuildHorizon(markid, w, ref f._faces[e2], f._e[e2], ref cf, ref ff);

-                                        }

-                                }

-                                return (ne);

-                        }

-

-                        public float EvaluatePD()

-                        {

-                                return EvaluatePD(_epaAccuracy);

-                        }

-

-                        private int[,] fidx;

-                        private int[,] eidx;

-

-                        public float EvaluatePD(float accuracy)

-                        {

-                                //Block* sablock = sa->BeginBlock();

-                                Face bestface = null;

-                                int markid = 1;

-                                _depth = -_infinity;

-                                _normal = new Vector3();

-                                _root = null;

-                                _nfaces = 0;

-                                _iterations = 0;

-                                _failed = false;

-                                /* Prepare hull         */

-                                if (_gjk.EncloseOrigin())

-                                {

-                                        int nfidx = 0;

-                                        int neidx = 0;

-                                        Gjk.MinkowskiVertice[] basemkv = new Gjk.MinkowskiVertice[5];

-                                        Face[] basefaces = new Face[6];

-                                        switch (_gjk.Order)

-                                        {

-                                                /* Tetrahedron          */

-                                                case 3:

-                                                        {

-                                                                fidx = new int[,] { { 2, 1, 0 }, { 3, 0, 1 }, { 3, 1, 2 }, { 3, 2, 0 } };

-                                                                eidx = new int[,] { { 0, 0, 2, 1 }, { 0, 1, 1, 1 }, { 0, 2, 3, 1 }, { 1, 0, 3, 2 }, { 2, 0, 1, 2 }, { 3, 0, 2, 2 } };

-                                                                nfidx = 4; neidx = 6;

-                                                        } break;

-                                                /* Hexahedron           */

-                                                case 4:

-                                                        {

-                                                                fidx = new int[,] { { 2, 0, 4 }, { 4, 1, 2 }, { 1, 4, 0 }, { 0, 3, 1 }, { 0, 2, 3 }, { 1, 3, 2 } };

-                                                                eidx = new int[,] { { 0, 0, 4, 0 }, { 0, 1, 2, 1 }, { 0, 2, 1, 2 }, { 1, 1, 5, 2 }, { 1, 0, 2, 0 }, { 2, 2, 3, 2 }, { 3, 1, 5, 0 }, { 3, 0, 4, 2 }, { 5, 1, 4, 1 } };

-                                                                nfidx = 6; neidx = 9;

-                                                        } break;

-                                        }

-                                        int i;

-

-                                        for (i = 0; i <= _gjk.Order; ++i)

-                                        {

-                                                //basemkv[i] = (GJK::Mkv*)sa->Allocate(sizeof(GJK::Mkv)); 

-                                                basemkv[i] = new Gjk.MinkowskiVertice();

-                                                basemkv[i].Vertice = _gjk.Simplex[i].Vertice;

-                                                basemkv[i].Ray = _gjk.Simplex[i].Ray;

-                                        }

-                                        for (i = 0; i < nfidx; ++i)

-                                        {

-                                                basefaces[i] = NewFace(basemkv[fidx[i, 0]], basemkv[fidx[i, 1]], basemkv[fidx[i, 2]]);

-                                        }

-                                        for (i = 0; i < neidx; ++i)

-                                        {

-                                                Link(ref basefaces[eidx[i, 0]], eidx[i, 1], ref basefaces[eidx[i, 2]], eidx[i, 3]);

-                                        }

-                                }

-                                if (0 == _nfaces)

-                                {

-                                        return _depth;

-                                }

-                                /* Expand hull          */

-                                for (; _iterations < _epaMaxIterations; ++_iterations)

-                                {

-                                        Face bf = FindBest();

-                                        if (bf != null)

-                                        {

-                                                Gjk.MinkowskiVertice w = Support(-bf._n);

-                                                float d = Vector3.Dot(bf._n, w.Vertice) + bf._d;

-                                                bestface = bf;

-                                                if (d < -accuracy)

-                                                {

-                                                        Face cf = null;

-                                                        Face ff = null;

-                                                        int nf = 0;

-                                                        Detach(ref bf);

-                                                        bf._mark = ++markid;

-                                                        for (int i = 0; i < 3; ++i)

-                                                        {

-                                                                nf += BuildHorizon(markid, w, ref bf._faces[i], bf._e[i], ref cf, ref ff);

-                                                        }

-                                                        if (nf <= 2) { break; }

-                                                        Link(ref cf, 1, ref ff, 2);

-                                                }

-                                                else break;

-                                        }

-                                        else break;

-                                }

-                                /* Extract contact      */

-                                if (bestface != null)

-                                {

-                                        Vector3 b = GetCoordinates(bestface);

-                                        _normal = bestface._n;

-                                        _depth = Max(0, bestface._d);

-                                        for (int i = 0; i < 2; ++i)

-                                        {

-                                                float s = i != 0 ? -1 : 1;

-                                                for (int j = 0; j < 3; ++j)

-                                                {

-                                                        _features[i, j] = _gjk.LocalSupport(s * bestface._vertices[j].Ray, i);

-                                                }

-                                        }

-                                        _nearest[0] = _features[0, 0] * b.X + _features[0, 1] * b.Y + _features[0, 2] * b.Z;

-                                        _nearest[1] = _features[1, 0] * b.X + _features[1, 1] * b.Y + _features[1, 2] * b.Z;

-                                }

-                                else _failed = true;

-                                return _depth;

-                        }

-

-                        private float Max(float a, float b)

-                        {

-                                return (a > b ? a : b);

-                        }

-

-                        private float Min(float a, float b)

-                        {

-                                return (a < b ? a : b);

-                        }

-                }

-        }

-}

+/*
+  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
+  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using MonoXnaCompactMaths;
+
+namespace XnaDevRu.BulletX
+{
+        /// <summary>
+        /// GJK-EPA collision solver by Nathanael Presson
+        /// Nov.2006
+        /// </summary>
+        public class GjkEpa
+        {
+                //private static readonly int _precision = 1 /* U(sizeof(F) == 4)*/;
+
+                private static readonly float _infinity = MathHelper.Infinity;
+                //private static readonly float _pi = (float)Math.PI;
+                private static readonly float _twoPi = (float)(Math.PI * 2);
+
+                private static readonly int _gjkMaxIterations = 128;
+                private static readonly int _gjkHashSize = 1 << 6;
+                private static readonly int _gjkHashMask = _gjkHashSize - 1;
+                private static readonly float _gjkInSimplexEpsilon = 0.0001f;
+                private static readonly float _gjkSquareInSimplexEpsilon = _gjkInSimplexEpsilon * _gjkInSimplexEpsilon;
+
+                private static readonly int _epaMaxIterations = 256;
+                private static readonly float _epaInFaceEpsilon = 0.01f;
+                private static readonly float _epaAccuracy = 0.001f;
+
+                public static float EpaAccuracy { get { return _epaAccuracy; } }
+
+                private static float Abs(float v) { return (v < 0 ? -v : v); }
+                private static float Sign(float v) { return (v < 0 ? -1 : 1); }
+
+                static void Swap<T>(ref T a, ref T b)
+                {
+                        T t = a;
+                        a = b;
+                        b = t;
+                }
+
+                public class Gjk
+                {
+                        public class MinkowskiVertice
+                        {
+                                private Vector3 _vertice;       /* Minkowski vertice    */
+                                private Vector3 _ray;           /* Ray                                  */
+
+                                public Vector3 Vertice { get { return _vertice; } set { _vertice = value; } }
+                                public Vector3 Ray { get { return _ray; } set { _ray = value; } }
+                        }
+                        public class He
+                        {
+                                private Vector3 _ray;
+                                private He _next;
+
+                                public He Next { get { return _next; } set { _next = value; } }
+                                public Vector3 Ray { get { return _ray; } set { _ray = value; } }
+                        }
+
+                        private He[] _table = new He[_gjkHashSize];
+                        private Matrix[] _wrotations = new Matrix[2];
+                        private Vector3[] _positions = new Vector3[2];
+                        private ConvexShape[] _shapes = new ConvexShape[2];
+                        private MinkowskiVertice[] _simplex = new MinkowskiVertice[5];
+                        private Vector3 _ray;
+                        private int _order;
+                        private int _iterations;
+                        private float _margin;
+                        private bool _failed;
+
+                        public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA,
+                                                Matrix wrotationB, Vector3 positionB, ConvexShape shapeB)
+                                : this(wrotationA, positionA, shapeA, wrotationB, positionB, shapeB, 0) { }
+
+                        public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA,
+                                                Matrix wrotationB, Vector3 positionB, ConvexShape shapeB,
+                                                float pmargin)
+                        {
+                for (int i = 0; i < _simplex.Length; i++)
+                    _simplex[i] = new MinkowskiVertice();
+
+                for (int i = 0; i < _wrotations.Length; i++)
+                    _wrotations[i] = new Matrix();
+
+                for (int i = 0; i < _positions.Length; i++)
+                    _positions[i] = new Vector3();
+
+                _wrotations[0] = wrotationA; _positions[0] = positionA; 
+                                _shapes[0] = shapeA;
+                _wrotations[0].Translation = Vector3.Zero;
+                                _wrotations[1] = wrotationB; _positions[1] = positionB; 
+                                _shapes[1] = shapeB;
+                _wrotations[1].Translation = Vector3.Zero;
+                                //sablock = sa->BeginBlock();
+                                _margin = pmargin;
+                                _failed = false;
+                        }
+
+                        public bool Failed { get { return _failed; } }
+                        public int Iterations { get { return _iterations; } }
+                        public int Order { get { return _order; } }
+                        public MinkowskiVertice[] Simplex { get { return _simplex; } }
+
+                        public int Hash(Vector3 v)
+                        {
+                                int h = ((int)(v.X * 15461) ^ (int)(v.Y * 83003) ^ (int)(v.Z * 15473));
+                                return (h * 169639) & _gjkHashMask;
+                        }
+
+                        public bool FetchSupport()
+                        {
+                                int h = Hash(_ray);
+                                He e = _table[h];
+                                while (e != null)
+                                {
+                                        if (e.Ray == _ray)
+                                        {
+                                                --_order;
+                                                return (false);
+                                        }
+                                        else e = e.Next;
+                                }
+                                e = new He(); 
+                                e.Ray = _ray; 
+                                e.Next = _table[h]; 
+                                _table[h] = e;
+                                Support(_ray, ref _simplex[++_order]);
+                                return (Vector3.Dot(_ray, _simplex[_order].Vertice) > 0);
+                        }
+
+                        public Vector3 LocalSupport(Vector3 d, int i)
+                        {
+                Matrix m = _wrotations[i];
+                m.Translation = Vector3.Zero;
+                Vector3 vtx = Vector3.TransformNormal(d, m);
+                Vector3 result = MathHelper.MatrixToVector(_wrotations[i], _shapes[i].LocalGetSupportingVertex(vtx));
+                return (result + _positions[i]);
+                        }
+
+                        public void Support(Vector3 d, ref MinkowskiVertice v)
+                        {
+                                v.Ray = d;
+                                v.Vertice = LocalSupport(d, 0) - LocalSupport(-d, 1) + d * _margin;
+                        }
+
+                        public bool SolveSimplex2(Vector3 ao, Vector3 ab)
+                        {
+                                if (Vector3.Dot(ab, ao) >= 0)
+                                {
+                                        Vector3 cabo = Vector3.Cross(ab, ao);
+                                        if (cabo.LengthSquared() > _gjkSquareInSimplexEpsilon)
+                                        { _ray = Vector3.Cross(cabo, ab); }
+                                        else
+                                        { return true; }
+                                }
+                                else
+                                {
+                    _order = 0;
+                                        _simplex[0].Ray = _simplex[1].Ray;
+                                        _simplex[0].Vertice = _simplex[1].Vertice;
+
+                    _ray = ao; 
+                }
+                                return false;
+                        }
+
+                        public bool SolveSimplex3(Vector3 ao, Vector3 ab, Vector3 ac)
+                        {
+                                return (SolveSimplex3a(ao, ab, ac, Vector3.Cross(ab, ac)));
+                        }
+
+                        public bool SolveSimplex3a(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 cabc)
+                        {
+                                if ((Vector3.Dot(Vector3.Cross(cabc, ab), ao)) < -_gjkInSimplexEpsilon)
+                                {
+                    _order = 1;
+                                        _simplex[0].Vertice = _simplex[1].Vertice;
+                                        _simplex[0].Ray = _simplex[1].Ray;
+
+                                        _simplex[1].Vertice = _simplex[2].Vertice;
+                                        _simplex[1].Ray = _simplex[2].Ray;
+
+                    return (SolveSimplex2(ao, ab));
+                }
+                                else if (Vector3.Dot(Vector3.Cross(cabc, ac), ao) > +_gjkInSimplexEpsilon)
+                                {
+                    _order = 1;
+                                        _simplex[1].Vertice = _simplex[2].Vertice;
+                                        _simplex[1].Ray = _simplex[2].Ray;
+
+                    return (SolveSimplex2(ao, ac));
+                }
+                                else
+                                {
+                                        float d = Vector3.Dot(cabc, ao);
+                                        if (Abs(d) > _gjkInSimplexEpsilon)
+                                        {
+                                                if (d > 0)
+                                                { _ray = cabc; }
+                                                else
+                                                { _ray = -cabc; Swap<MinkowskiVertice>(ref _simplex[0], ref _simplex[1]); }
+                                                return (false);
+                                        }
+                                        else return (true);
+                                }
+                        }
+
+                        public bool SolveSimplex4(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 ad)
+                        {
+                                Vector3 crs;
+                                if (Vector3.Dot((crs = Vector3.Cross(ab, ac)), ao) > _gjkInSimplexEpsilon)
+                                {
+                                        _order = 2;
+                                        _simplex[0].Vertice = _simplex[1].Vertice;
+                                        _simplex[0].Ray = _simplex[1].Ray;
+
+                                        _simplex[1].Vertice = _simplex[2].Vertice;
+                                        _simplex[1].Ray = _simplex[2].Ray;
+
+                                        _simplex[2].Vertice = _simplex[3].Vertice;
+                                        _simplex[2].Ray = _simplex[3].Ray;
+                    
+                    return (SolveSimplex3a(ao, ab, ac, crs));
+                                }
+                                else if (Vector3.Dot((crs = Vector3.Cross(ac, ad)), ao) > _gjkInSimplexEpsilon)
+                                { 
+                    _order = 2;
+                                        _simplex[2].Vertice = _simplex[3].Vertice;
+                                        _simplex[2].Ray = _simplex[3].Ray;
+                    
+                    return (SolveSimplex3a(ao, ac, ad, crs)); 
+                }
+                                else if (Vector3.Dot((crs = Vector3.Cross(ad, ab)), ao) > _gjkInSimplexEpsilon)
+                                {
+                                        _order = 2;
+
+                                        _simplex[1].Vertice = _simplex[0].Vertice;
+                                        _simplex[1].Ray = _simplex[0].Ray;
+
+                                        _simplex[0].Vertice = _simplex[2].Vertice;
+                                        _simplex[0].Ray = _simplex[2].Ray;
+
+                                        _simplex[2].Vertice = _simplex[3].Vertice;
+                                        _simplex[2].Ray = _simplex[3].Ray; 
+                    
+                    return (SolveSimplex3a(ao, ad, ab, crs));
+                                }
+                                else return (true);
+                        }
+
+                        public bool SearchOrigin()
+                        {
+                                return SearchOrigin(new Vector3(1, 0, 0));
+                        }
+
+                        public bool SearchOrigin(Vector3 initray)
+                        {
+                                _iterations = 0;
+                                unchecked
+                                {
+                                        _order = (int)(-1);
+                                }
+                                _failed = false;
+                                _ray = Vector3.Normalize(initray);
+
+                                //ClearMemory(table, sizeof(void*) * GJK_hashsize);
+                for (int i = 0; i < _table.Length; i++)
+                    _table[i] = null;
+                                FetchSupport();
+                                _ray = -_simplex[0].Vertice;
+                                for (; _iterations < _gjkMaxIterations; ++_iterations)
+                                {
+                                        float rl = _ray.Length();
+                                        _ray /= rl > 0 ? rl : 1;
+                                        if (FetchSupport())
+                                        {
+                                                bool found = (false);
+                                                switch (_order)
+                                                {
+                                                        case 1: found = SolveSimplex2(-_simplex[1].Vertice, _simplex[0].Vertice - _simplex[1].Vertice); break;
+                                                        case 2: found = SolveSimplex3(-_simplex[2].Vertice, _simplex[1].Vertice - _simplex[2].Vertice, _simplex[0].Vertice - _simplex[2].Vertice); break;
+                                                        case 3: found = SolveSimplex4(-_simplex[3].Vertice, _simplex[2].Vertice - _simplex[3].Vertice, _simplex[1].Vertice - _simplex[3].Vertice, _simplex[0].Vertice - _simplex[3].Vertice); break;
+                                                }
+                                                if (found) return (true);
+                                        }
+                                        else return (false);
+                                }
+                                _failed = true;
+                                return (false);
+                        }
+
+                        public bool EncloseOrigin()
+                        {
+                                switch (_order)
+                                {
+                                        /* Point                */
+                                        case 0: break;
+                                        /* Line                 */
+                                        case 1:
+                                                Vector3 ab = _simplex[1].Vertice - _simplex[0].Vertice;
+                                                Vector3[] b ={  Vector3.Cross(ab, new Vector3(1, 0, 0)),
+                                                                                        Vector3.Cross(ab, new Vector3(0, 1, 0)),
+                                                                                        Vector3.Cross(ab, new Vector3(0, 0, 1)) };
+                                                float[] m ={ b[0].LengthSquared(), b[1].LengthSquared(), b[2].LengthSquared() };
+                                                Matrix r = Matrix.CreateFromQuaternion(new Quaternion(Vector3.Normalize(ab), _twoPi / 3));
+                                                Vector3 w = b[m[0] > m[1] ? m[0] > m[2] ? 0 : 2 : m[1] > m[2] ? 1 : 2];
+                                                Support(Vector3.Normalize(w), ref _simplex[4]); w = Vector3.TransformNormal(w, r);
+                        Support(Vector3.Normalize(w), ref _simplex[2]); w = Vector3.TransformNormal(w, r);
+                        Support(Vector3.Normalize(w), ref _simplex[3]); w = Vector3.TransformNormal(w, r);
+                                                _order = 4;
+                                                return true;
+                                        /* Triangle             */
+                                        case 2:
+                                                Vector3 n = Vector3.Normalize(Vector3.Cross(_simplex[1].Vertice - _simplex[0].Vertice, _simplex[2].Vertice - _simplex[0].Vertice));
+                                                Support(n, ref _simplex[3]);
+                                                Support(-n, ref _simplex[4]);
+                                                _order = 4;
+                                                return true;
+                                        /* Tetrahedron  */
+                                        case 3: return (true);
+                                        /* Hexahedron   */
+                                        case 4: return (true);
+                                }
+                                return (false);
+                        }
+                }
+
+                public class Epa
+                {
+                        public class Face
+                        {
+                                public Gjk.MinkowskiVertice[] _vertices = new Gjk.MinkowskiVertice[3];
+                                public Face[] _faces = new Face[3];
+                                public int[] _e = new int[3];
+                                public Vector3 _n;
+                                public float _d;
+                                public int _mark;
+                                public Face _prev;
+                                public Face _next;
+                        }
+
+                        private Gjk _gjk;
+                        private Face _root;
+                        private int _nfaces;
+                        private int _iterations;
+                        private Vector3[,] _features = new Vector3[2, 3];
+                        private Vector3[] _nearest = new Vector3[2];
+                        private Vector3 _normal;
+                        private float _depth;
+                        private bool _failed;
+
+                        public Epa(Gjk gjk)
+                        {
+                                this._gjk = gjk;
+                        }
+
+                        public bool Failed { get { return _failed; } }
+                        public int Iterations { get { return _iterations; } }
+                        public Vector3 Normal { get { return _normal; } }
+                        public Vector3[] Nearest { get { return _nearest; } }
+
+                        public Vector3 GetCoordinates(Face face)
+                        {
+                                Vector3 o = face._n * -face._d;
+                                float[] a ={    Vector3.Cross(face._vertices[0].Vertice - o, face._vertices[1].Vertice - o).Length(),
+                                                                Vector3.Cross(face._vertices[1].Vertice - o, face._vertices[2].Vertice - o).Length(),
+                                                                Vector3.Cross(face._vertices[2].Vertice - o, face._vertices[0].Vertice - o).Length()};
+                                float sm = a[0] + a[1] + a[2];
+                                return (new Vector3(a[1], a[2], a[0]) / (sm > 0 ? sm : 1));
+                        }
+
+                        public Face FindBest()
+                        {
+                                Face bf = null;
+                                if (_root != null)
+                                {
+                                        Face cf = _root;
+                                        float bd = _infinity;
+                                        do
+                                        {
+                                                if (cf._d < bd) { bd = cf._d; bf = cf; }
+                                        } while (null != (cf = cf._next));
+                                }
+                                return bf;
+                        }
+
+                        public bool Set(ref Face f, Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c)
+                        {
+                                Vector3 nrm = Vector3.Cross(b.Vertice - a.Vertice, c.Vertice - a.Vertice);
+                                float len = nrm.Length();
+                                bool valid = (Vector3.Dot(Vector3.Cross(a.Vertice, b.Vertice), nrm) >= -_epaInFaceEpsilon &&
+                                                                 Vector3.Dot(Vector3.Cross(b.Vertice, c.Vertice), nrm) >= -_epaInFaceEpsilon &&
+                                                                 Vector3.Dot(Vector3.Cross(c.Vertice, a.Vertice), nrm) >= -_epaInFaceEpsilon);
+                                f._vertices[0] = a;
+                                f._vertices[1] = b;
+                                f._vertices[2] = c;
+                                f._mark = 0;
+                                f._n = nrm / (len > 0 ? len : _infinity);
+                                f._d = Max(0, -Vector3.Dot(f._n, a.Vertice));
+                                return valid;
+                        }
+
+                        public Face NewFace(Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c)
+                        {
+                                Face pf = new Face();
+                                if (Set(ref pf, a, b, c))
+                                {
+                                        if (_root != null) _root._prev = pf;
+                                        pf._prev = null;
+                                        pf._next = _root;
+                                        _root = pf;
+                                        ++_nfaces;
+                                }
+                                else
+                                {
+                                        pf._prev = pf._next = null;
+                                }
+                                return (pf);
+                        }
+
+                        public void Detach(ref Face face)
+                        {
+                                if (face._prev != null || face._next != null)
+                                {
+                                        --_nfaces;
+                                        if (face == _root)
+                                        {
+                                                _root = face._next;
+                                                _root._prev = null;
+                                        }
+                                        else
+                                        {
+                                                if (face._next == null)
+                                                {
+                                                        face._prev._next = null;
+                                                }
+                                                else
+                                                {
+                                                        face._prev._next = face._next;
+                                                        face._next._prev = face._prev;
+                                                }
+                                        }
+                                        face._prev = face._next = null;
+                                }
+                        }
+
+                        public void Link(ref Face f0, int e0, ref Face f1, int e1)
+                        {
+                                f0._faces[e0] = f1; f1._e[e1] = e0;
+                                f1._faces[e1] = f0; f0._e[e0] = e1;
+                        }
+
+                        public Gjk.MinkowskiVertice Support(Vector3 w)
+                        {
+                                Gjk.MinkowskiVertice v = new Gjk.MinkowskiVertice();
+                                _gjk.Support(w, ref v);
+                                return v;
+                        }
+
+                        private static int[] mod3 ={ 0, 1, 2, 0, 1 };
+
+                        public int BuildHorizon(int markid, Gjk.MinkowskiVertice w, ref Face f, int e, ref Face cf, ref Face ff)
+                        {
+                                int ne = (0);
+                                if (f._mark != markid)
+                                {
+                                        int e1 = (mod3[e + 1]);
+                                        if ((Vector3.Dot(f._n, w.Vertice) + f._d) > 0)
+                                        {
+                                                Face nf = NewFace(f._vertices[e1], f._vertices[e], w);
+                                                Link(ref nf, 0, ref f, e);
+                                                if (cf != null) Link(ref cf, 1, ref nf, 2); else ff = nf;
+                                                cf = nf; ne = 1;
+                                        }
+                                        else
+                                        {
+                                                int e2 = (mod3[e + 2]);
+                                                Detach(ref f);
+                                                f._mark = markid;
+                                                ne += BuildHorizon(markid, w, ref f._faces[e1], f._e[e1], ref cf, ref ff);
+                                                ne += BuildHorizon(markid, w, ref f._faces[e2], f._e[e2], ref cf, ref ff);
+                                        }
+                                }
+                                return (ne);
+                        }
+
+                        public float EvaluatePD()
+                        {
+                                return EvaluatePD(_epaAccuracy);
+                        }
+
+                        private int[,] fidx;
+                        private int[,] eidx;
+
+                        public float EvaluatePD(float accuracy)
+                        {
+                                //Block* sablock = sa->BeginBlock();
+                                Face bestface = null;
+                                int markid = 1;
+                                _depth = -_infinity;
+                                _normal = new Vector3();
+                                _root = null;
+                                _nfaces = 0;
+                                _iterations = 0;
+                                _failed = false;
+                                /* Prepare hull         */
+                                if (_gjk.EncloseOrigin())
+                                {
+                                        int nfidx = 0;
+                                        int neidx = 0;
+                                        Gjk.MinkowskiVertice[] basemkv = new Gjk.MinkowskiVertice[5];
+                                        Face[] basefaces = new Face[6];
+                                        switch (_gjk.Order)
+                                        {
+                                                /* Tetrahedron          */
+                                                case 3:
+                                                        {
+                                                                fidx = new int[,] { { 2, 1, 0 }, { 3, 0, 1 }, { 3, 1, 2 }, { 3, 2, 0 } };
+                                                                eidx = new int[,] { { 0, 0, 2, 1 }, { 0, 1, 1, 1 }, { 0, 2, 3, 1 }, { 1, 0, 3, 2 }, { 2, 0, 1, 2 }, { 3, 0, 2, 2 } };
+                                                                nfidx = 4; neidx = 6;
+                                                        } break;
+                                                /* Hexahedron           */
+                                                case 4:
+                                                        {
+                                                                fidx = new int[,] { { 2, 0, 4 }, { 4, 1, 2 }, { 1, 4, 0 }, { 0, 3, 1 }, { 0, 2, 3 }, { 1, 3, 2 } };
+                                                                eidx = new int[,] { { 0, 0, 4, 0 }, { 0, 1, 2, 1 }, { 0, 2, 1, 2 }, { 1, 1, 5, 2 }, { 1, 0, 2, 0 }, { 2, 2, 3, 2 }, { 3, 1, 5, 0 }, { 3, 0, 4, 2 }, { 5, 1, 4, 1 } };
+                                                                nfidx = 6; neidx = 9;
+                                                        } break;
+                                        }
+                                        int i;
+
+                                        for (i = 0; i <= _gjk.Order; ++i)
+                                        {
+                                                //basemkv[i] = (GJK::Mkv*)sa->Allocate(sizeof(GJK::Mkv)); 
+                                                basemkv[i] = new Gjk.MinkowskiVertice();
+                                                basemkv[i].Vertice = _gjk.Simplex[i].Vertice;
+                                                basemkv[i].Ray = _gjk.Simplex[i].Ray;
+                                        }
+                                        for (i = 0; i < nfidx; ++i)
+                                        {
+                                                basefaces[i] = NewFace(basemkv[fidx[i, 0]], basemkv[fidx[i, 1]], basemkv[fidx[i, 2]]);
+                                        }
+                                        for (i = 0; i < neidx; ++i)
+                                        {
+                                                Link(ref basefaces[eidx[i, 0]], eidx[i, 1], ref basefaces[eidx[i, 2]], eidx[i, 3]);
+                                        }
+                                }
+                                if (0 == _nfaces)
+                                {
+                                        return _depth;
+                                }
+                                /* Expand hull          */
+                                for (; _iterations < _epaMaxIterations; ++_iterations)
+                                {
+                                        Face bf = FindBest();
+                                        if (bf != null)
+                                        {
+                                                Gjk.MinkowskiVertice w = Support(-bf._n);
+                                                float d = Vector3.Dot(bf._n, w.Vertice) + bf._d;
+                                                bestface = bf;
+                                                if (d < -accuracy)
+                                                {
+                                                        Face cf = null;
+                                                        Face ff = null;
+                                                        int nf = 0;
+                                                        Detach(ref bf);
+                                                        bf._mark = ++markid;
+                                                        for (int i = 0; i < 3; ++i)
+                                                        {
+                                                                nf += BuildHorizon(markid, w, ref bf._faces[i], bf._e[i], ref cf, ref ff);
+                                                        }
+                                                        if (nf <= 2) { break; }
+                                                        Link(ref cf, 1, ref ff, 2);
+                                                }
+                                                else break;
+                                        }
+                                        else break;
+                                }
+                                /* Extract contact      */
+                                if (bestface != null)
+                                {
+                                        Vector3 b = GetCoordinates(bestface);
+                                        _normal = bestface._n;
+                                        _depth = Max(0, bestface._d);
+                                        for (int i = 0; i < 2; ++i)
+                                        {
+                                                float s = i != 0 ? -1 : 1;
+                                                for (int j = 0; j < 3; ++j)
+                                                {
+                                                        _features[i, j] = _gjk.LocalSupport(s * bestface._vertices[j].Ray, i);
+                                                }
+                                        }
+                                        _nearest[0] = _features[0, 0] * b.X + _features[0, 1] * b.Y + _features[0, 2] * b.Z;
+                                        _nearest[1] = _features[1, 0] * b.X + _features[1, 1] * b.Y + _features[1, 2] * b.Z;
+                                }
+                                else _failed = true;
+                                return _depth;
+                        }
+
+                        private float Max(float a, float b)
+                        {
+                                return (a > b ? a : b);
+                        }
+
+                        private float Min(float a, float b)
+                        {
+                                return (a < b ? a : b);
+                        }
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs
index 739b4fc..551449f 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs
@@ -1,56 +1,56 @@
-/*

-  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru

-  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

-

-  This software is provided 'as-is', without any express or implied

-  warranty.  In no event will the authors be held liable for any damages

-  arising from the use of this software.

-

-  Permission is granted to anyone to use this software for any purpose,

-  including commercial applications, and to alter it and redistribute it

-  freely, subject to the following restrictions:

-

-  1. The origin of this software must not be misrepresented; you must not

-     claim that you wrote the original software. If you use this software

-     in a product, an acknowledgment in the product documentation would be

-     appreciated but is not required.

-  2. Altered source versions must be plainly marked as such, and must not be

-     misrepresented as being the original software.

-  3. This notice may not be removed or altered from any source distribution.

-*/

-

-using System;

-using System.Collections.Generic;

-using System.Text;

-using MonoXnaCompactMaths;

-

-namespace XnaDevRu.BulletX

-{

-        /// <summary>

-        /// EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to

-        /// calculate the penetration depth between two convex shapes.

-        /// </summary>

-        public class GjkEpaPenetrationDepthSolver : IConvexPenetrationDepthSolver

-        {

-                public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, ConvexShape convexA, ConvexShape convexB, Matrix transformA, Matrix transformB, Vector3 vector, out Vector3 ptrA, out Vector3 ptrB, IDebugDraw debugDraw)

-                {

-                        float radialmargin = 0;

-

-                        GjkEpaSolver.Results results;

-                        if (GjkEpaSolver.Collide(convexA, transformA,

-                                                                         convexB, transformB,

-                                                                         radialmargin, out results))

-                        {

-                                //      debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));

-                                //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);

-                                ptrA = results.Witnesses[0];

-                                ptrB = results.Witnesses[1];

-                                return true;

-                        }

-                        ptrA = new Vector3();

-                        ptrB = new Vector3();

-

-                        return false;

-                }

-        }

-}

+/*
+  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
+  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using MonoXnaCompactMaths;
+
+namespace XnaDevRu.BulletX
+{
+        /// <summary>
+        /// EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to
+        /// calculate the penetration depth between two convex shapes.
+        /// </summary>
+        public class GjkEpaPenetrationDepthSolver : IConvexPenetrationDepthSolver
+        {
+                public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, ConvexShape convexA, ConvexShape convexB, Matrix transformA, Matrix transformB, Vector3 vector, out Vector3 ptrA, out Vector3 ptrB, IDebugDraw debugDraw)
+                {
+                        float radialmargin = 0;
+
+                        GjkEpaSolver.Results results;
+                        if (GjkEpaSolver.Collide(convexA, transformA,
+                                                                         convexB, transformB,
+                                                                         radialmargin, out results))
+                        {
+                                //      debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
+                                //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
+                                ptrA = results.Witnesses[0];
+                                ptrB = results.Witnesses[1];
+                                return true;
+                        }
+                        ptrA = new Vector3();
+                        ptrB = new Vector3();
+
+                        return false;
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs
index aa9d61e..20af192 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs
@@ -1,101 +1,101 @@
-/*

-  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru

-  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

-

-  This software is provided 'as-is', without any express or implied

-  warranty.  In no event will the authors be held liable for any damages

-  arising from the use of this software.

-

-  Permission is granted to anyone to use this software for any purpose,

-  including commercial applications, and to alter it and redistribute it

-  freely, subject to the following restrictions:

-

-  1. The origin of this software must not be misrepresented; you must not

-     claim that you wrote the original software. If you use this software

-     in a product, an acknowledgment in the product documentation would be

-     appreciated but is not required.

-  2. Altered source versions must be plainly marked as such, and must not be

-     misrepresented as being the original software.

-  3. This notice may not be removed or altered from any source distribution.

-*/

-

-using System;

-using System.Collections.Generic;

-using System.Text;

-using MonoXnaCompactMaths;

-

-namespace XnaDevRu.BulletX

-{

-        /// <summary>

-        /// GjkEpaSolver contributed under zlib by Nathanael Presson

-        /// </summary>

-        public class GjkEpaSolver

-        {

-                public struct Results

-                {

-                        public enum Status

-                        {

-                                Separated,              /* Shapes doesnt penetrate                                                                                              */

-                                Penetrating,    /* Shapes are penetrating                                                                                               */

-                                GjkFailed,              /* GJK phase fail, no big issue, shapes are probably just 'touching'    */

-                                EpaFailed,              /* EPA phase fail, bigger problem, need to save parameters, and debug   */

-                        }

-

-                        private Vector3[] _witnesses;

-                        private Vector3 _normal;

-                        private float _depth;

-                        private int _epaIterations;

-                        private int _gjkIterations;

-                        private Status _status;

-

-                        public Vector3[] Witnesses { get { return _witnesses; } set { _witnesses = value; } }

-                        public Vector3 Normal { get { return _normal; } set { _normal = value; } }

-                        public float Depth { get { return _depth; } set { _depth = value; } }

-                        public int EpaIterations { get { return _epaIterations; } set { _epaIterations = value; } }

-                        public int GjkIterations { get { return _gjkIterations; } set { _gjkIterations = value; } }

-                        public Status ResultStatus { get { return _status; } set { _status = value; } }

-                }

-

-                public static bool Collide(ConvexShape shapeA, Matrix wtrsA,

-                                        ConvexShape shapeB, Matrix wtrsB,

-                                        float radialmargin,

-                                        out Results results)

-                {

-                        /* Initialize                                   */

-                        results = new Results();

-                        results.Witnesses = new Vector3[2];

-                        results.Witnesses[0] =

-                        results.Witnesses[1] =

-                        results.Normal = new Vector3();

-                        results.Depth = 0;

-                        results.ResultStatus = Results.Status.Separated;

-                        results.EpaIterations = 0;

-                        results.GjkIterations = 0;

-                        /* Use GJK to locate origin             */

-                        GjkEpa.Gjk gjk = new GjkEpa.Gjk(wtrsA, wtrsA.Translation, shapeA,

-                                                        wtrsB, wtrsB.Translation, shapeB,

-                                                        radialmargin + GjkEpa.EpaAccuracy);

-                        bool collide = gjk.SearchOrigin();

-                        results.GjkIterations = gjk.Iterations + 1;

-                        if (collide)

-                        {

-                                /* Then EPA for penetration depth       */

-                                GjkEpa.Epa epa = new GjkEpa.Epa(gjk);

-                                float pd = epa.EvaluatePD();

-                                results.EpaIterations = epa.Iterations + 1;

-                                if (pd > 0)

-                                {

-                                        results.ResultStatus = Results.Status.Penetrating;

-                                        results.Normal = epa.Normal;

-                                        results.Depth = pd;

-                                        results.Witnesses[0] = epa.Nearest[0];

-                                        results.Witnesses[1] = epa.Nearest[1];

-                                        return true;

-                                }

-                                else { if (epa.Failed) results.ResultStatus = Results.Status.EpaFailed; }

-                        }

-                        else { if (gjk.Failed) results.ResultStatus = Results.Status.GjkFailed; }

-                        return false;

-                }

-        }

-}

+/*
+  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
+  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using MonoXnaCompactMaths;
+
+namespace XnaDevRu.BulletX
+{
+        /// <summary>
+        /// GjkEpaSolver contributed under zlib by Nathanael Presson
+        /// </summary>
+        public class GjkEpaSolver
+        {
+                public struct Results
+                {
+                        public enum Status
+                        {
+                                Separated,              /* Shapes doesnt penetrate                                                                                              */
+                                Penetrating,    /* Shapes are penetrating                                                                                               */
+                                GjkFailed,              /* GJK phase fail, no big issue, shapes are probably just 'touching'    */
+                                EpaFailed,              /* EPA phase fail, bigger problem, need to save parameters, and debug   */
+                        }
+
+                        private Vector3[] _witnesses;
+                        private Vector3 _normal;
+                        private float _depth;
+                        private int _epaIterations;
+                        private int _gjkIterations;
+                        private Status _status;
+
+                        public Vector3[] Witnesses { get { return _witnesses; } set { _witnesses = value; } }
+                        public Vector3 Normal { get { return _normal; } set { _normal = value; } }
+                        public float Depth { get { return _depth; } set { _depth = value; } }
+                        public int EpaIterations { get { return _epaIterations; } set { _epaIterations = value; } }
+                        public int GjkIterations { get { return _gjkIterations; } set { _gjkIterations = value; } }
+                        public Status ResultStatus { get { return _status; } set { _status = value; } }
+                }
+
+                public static bool Collide(ConvexShape shapeA, Matrix wtrsA,
+                                        ConvexShape shapeB, Matrix wtrsB,
+                                        float radialmargin,
+                                        out Results results)
+                {
+                        /* Initialize                                   */
+                        results = new Results();
+                        results.Witnesses = new Vector3[2];
+                        results.Witnesses[0] =
+                        results.Witnesses[1] =
+                        results.Normal = new Vector3();
+                        results.Depth = 0;
+                        results.ResultStatus = Results.Status.Separated;
+                        results.EpaIterations = 0;
+                        results.GjkIterations = 0;
+                        /* Use GJK to locate origin             */
+                        GjkEpa.Gjk gjk = new GjkEpa.Gjk(wtrsA, wtrsA.Translation, shapeA,
+                                                        wtrsB, wtrsB.Translation, shapeB,
+                                                        radialmargin + GjkEpa.EpaAccuracy);
+                        bool collide = gjk.SearchOrigin();
+                        results.GjkIterations = gjk.Iterations + 1;
+                        if (collide)
+                        {
+                                /* Then EPA for penetration depth       */
+                                GjkEpa.Epa epa = new GjkEpa.Epa(gjk);
+                                float pd = epa.EvaluatePD();
+                                results.EpaIterations = epa.Iterations + 1;
+                                if (pd > 0)
+                                {
+                                        results.ResultStatus = Results.Status.Penetrating;
+                                        results.Normal = epa.Normal;
+                                        results.Depth = pd;
+                                        results.Witnesses[0] = epa.Nearest[0];
+                                        results.Witnesses[1] = epa.Nearest[1];
+                                        return true;
+                                }
+                                else { if (epa.Failed) results.ResultStatus = Results.Status.EpaFailed; }
+                        }
+                        else { if (gjk.Failed) results.ResultStatus = Results.Status.GjkFailed; }
+                        return false;
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs
index 0831ff5..7557ed9 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs
@@ -1,343 +1,343 @@
-/*

-  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru

-  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

-

-  This software is provided 'as-is', without any express or implied

-  warranty.  In no event will the authors be held liable for any damages

-  arising from the use of this software.

-

-  Permission is granted to anyone to use this software for any purpose,

-  including commercial applications, and to alter it and redistribute it

-  freely, subject to the following restrictions:

-

-  1. The origin of this software must not be misrepresented; you must not

-     claim that you wrote the original software. If you use this software

-     in a product, an acknowledgment in the product documentation would be

-     appreciated but is not required.

-  2. Altered source versions must be plainly marked as such, and must not be

-     misrepresented as being the original software.

-  3. This notice may not be removed or altered from any source distribution.

-*/

-

-using System;

-using System.Collections.Generic;

-using System.Text;

-using MonoXnaCompactMaths;

-

-namespace XnaDevRu.BulletX

-{

-        public class GjkPairDetector : DiscreteCollisionDetectorInterface

-        {

-                private Vector3 _cachedSeparatingAxis;

-                private IConvexPenetrationDepthSolver _penetrationDepthSolver;

-                private ISimplexSolver _simplexSolver;

-                private ConvexShape _minkowskiA, _minkowskiB;

-                private bool _ignoreMargin;

-

-                private int _lastUsedMethod;

-                private int _currentIteration;

-                private int _degenerateSimplex;

-                private int _catchDegeneracies;

-

-                private static int _numDeepPenetrationChecks = 0;

-                private static int _numGjkChecks = 0;

-

-                private const float RelativeError2 = 1.0e-6f;

-

-                #region Properties

-                public int LastUsedMethod

-                {

-                        get { return _lastUsedMethod; }

-                        set { _lastUsedMethod = value; }

-                }

-

-                public int CurrentIteration

-                {

-                        get { return _currentIteration; }

-                        set { _currentIteration = value; }

-                }

-

-                public int DegenerateSimplex

-                {

-                        get { return _degenerateSimplex; }

-                        set { _degenerateSimplex = value; }

-                }

-

-                public int CatchDegeneracies

-                {

-                        get { return _catchDegeneracies; }

-                        set { _catchDegeneracies = value; }

-                }

-

-                public static int DeepPenetrationChecksCount { get { return _numDeepPenetrationChecks; } }

-                public static int GjkChecksCount { get { return _numGjkChecks; } }

-                #endregion

-

-                public GjkPairDetector(ConvexShape objectA, ConvexShape objectB,

-                                                           ISimplexSolver simplexSolver,

-                                                           IConvexPenetrationDepthSolver penetrationDepthSolver)

-                {

-                        _cachedSeparatingAxis = new Vector3(0, 0, 1);

-

-                        _penetrationDepthSolver = penetrationDepthSolver;

-                        _simplexSolver = simplexSolver;

-                        _minkowskiA = objectA;

-                        _minkowskiB = objectB;

-                        _ignoreMargin = false;

-                        _lastUsedMethod = -1;

-                        _catchDegeneracies = 1;

-                }

-

-                public void setMinkowskiA(ConvexShape minkA)

-                {

-                        _minkowskiA = minkA;

-                }

-

-                public void setMinkowskiB(ConvexShape minkB)

-                {

-                        _minkowskiB = minkB;

-                }

-                public void setCachedSeperatingAxis(Vector3 seperatingAxis)

-                {

-                        _cachedSeparatingAxis = seperatingAxis;

-                }

-

-                public void setPenetrationDepthSolver(IConvexPenetrationDepthSolver penetrationDepthSolver)

-                {

-                        this._penetrationDepthSolver = penetrationDepthSolver;

-                }

-

-                public void setIgnoreMargin(bool ignoreMargin)

-                {

-                        this._ignoreMargin = ignoreMargin;

-                }

-

-                public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw)

-                {

-                        float distance = 0;

-

-                        Vector3 normalInB = new Vector3();

-                        Vector3 pointOnA = new Vector3(), pointOnB = new Vector3();

-

-                        Matrix localTransA = input.TransformA;

-                        Matrix localTransB = input.TransformB;

-

-                        Vector3 positionOffset = (localTransA.Translation + localTransB.Translation) * 0.5f;

-                        localTransA.Translation -= positionOffset;

-                        localTransB.Translation -= positionOffset;

-

-                        float marginA = _minkowskiA.Margin;

-                        float marginB = _minkowskiB.Margin;

-

-                        _numGjkChecks++;

-

-                        if (_ignoreMargin)

-                        {

-                                marginA = 0;

-                                marginB = 0;

-                        }

-

-                        _currentIteration = 0;

-

-                        int gjkMaxIter = 1000;

-                        _cachedSeparatingAxis = new Vector3(0, 1, 0);

-

-                        bool isValid = false;

-                        bool checkSimplex = false;

-                        bool checkPenetration = true;

-                        _degenerateSimplex = 0;

-

-                        _lastUsedMethod = -1;

-

-                        {

-                                float squaredDistance = MathHelper.Infinity;

-                                float delta = 0;

-

-                                float margin = marginA + marginB;

-

-                                _simplexSolver.Reset();

-

-                                while (true)

-                                {

-                                        Matrix transABasis = input.TransformA;

-                                        transABasis.Translation = Vector3.Zero;

-

-                                        Matrix transBBasis = input.TransformB;

-                                        transBBasis.Translation = Vector3.Zero;

-

-                                        Vector3 seperatingAxisInA = Vector3.TransformNormal(-_cachedSeparatingAxis, transABasis);

-                                        Vector3 seperatingAxisInB = Vector3.TransformNormal(_cachedSeparatingAxis, transBBasis);

-

-                                        Vector3 pInA = _minkowskiA.LocalGetSupportingVertexWithoutMargin(seperatingAxisInA);

-                                        Vector3 qInB = _minkowskiB.LocalGetSupportingVertexWithoutMargin(seperatingAxisInB);

-                                        Vector3 pWorld = MathHelper.MatrixToVector(localTransA, pInA);

-                                        Vector3 qWorld = MathHelper.MatrixToVector(localTransB, qInB);

-

-                                        Vector3 w = pWorld - qWorld;

-                                        delta = Vector3.Dot(_cachedSeparatingAxis, w);

-

-                                        if ((delta > 0.0) && (delta * delta > squaredDistance * input.MaximumDistanceSquared))

-                                        {

-                                                checkPenetration = false;

-                                                break;

-                                        }

-

-                                        if (_simplexSolver.InSimplex(w))

-                                        {

-                                                _degenerateSimplex = 1;

-                                                checkSimplex = true;

-                                                break;

-                                        }

-

-                                        float f0 = squaredDistance - delta;

-                                        float f1 = squaredDistance * RelativeError2;

-

-                                        if (f0 <= f1)

-                                        {

-                                                if (f0 <= 0.0f)

-                                                {

-                                                        _degenerateSimplex = 2;

-                                                }

-

-                                                checkSimplex = true;

-                                                break;

-                                        }

-

-                                        _simplexSolver.AddVertex(w, pWorld, qWorld);

-

-                                        if (!_simplexSolver.Closest(out _cachedSeparatingAxis))

-                                        {

-                                                _degenerateSimplex = 3;

-                                                checkSimplex = true;

-                                                break;

-                                        }

-

-                                        float previouseSquaredDistance = squaredDistance;

-                                        squaredDistance = _cachedSeparatingAxis.LengthSquared();

-

-                                        if (previouseSquaredDistance - squaredDistance <= MathHelper.Epsilon * previouseSquaredDistance)

-                                        {

-                                                _simplexSolver.BackupClosest(out _cachedSeparatingAxis);

-                                                checkSimplex = true;

-                                                break;

-                                        }

-

-                                        if (_currentIteration++ > gjkMaxIter)

-                                        {

-#if DEBUG

-                                                Console.WriteLine("GjkPairDetector maxIter exceeded: {0}", _currentIteration);

-                                                Console.WriteLine("sepAxis=({0},{1},{2}), squaredDistance = {3}, shapeTypeA={4}, shapeTypeB={5}",

-                                                        _cachedSeparatingAxis.X,

-                                                        _cachedSeparatingAxis.Y,

-                                                        _cachedSeparatingAxis.Z,

-                                                        squaredDistance,

-                                                        _minkowskiA.ShapeType,

-                                                        _minkowskiB.ShapeType

-                                                );

-#endif

-                                                break;

-                                        }

-

-                                        bool check = (!_simplexSolver.FullSimplex);

-

-                                        if (!check)

-                                        {

-                                                _simplexSolver.BackupClosest(out _cachedSeparatingAxis);

-                                                break;

-                                        }

-                                }

-

-                                if (checkSimplex)

-                                {

-                                        _simplexSolver.ComputePoints(out pointOnA, out pointOnB);

-                                        normalInB = pointOnA - pointOnB;

-                                        float lenSqr = _cachedSeparatingAxis.LengthSquared();

-

-                                        if (lenSqr < 0.0001f)

-                                        {

-                                                _degenerateSimplex = 5;

-                                        }

-

-                                        if (lenSqr > MathHelper.Epsilon * MathHelper.Epsilon)

-                                        {

-                                                float rlen = 1.0f / (float)Math.Sqrt((float)lenSqr);

-                                                normalInB *= rlen;

-                                                float s = (float)Math.Sqrt((float)squaredDistance);

-

-                                                BulletDebug.Assert(s > 0);

-                                                pointOnA -= _cachedSeparatingAxis * (marginA / s);

-                                                pointOnB += _cachedSeparatingAxis * (marginB / s);

-                                                distance = ((1 / rlen) - margin);

-

-                                                isValid = true;

-

-                                                _lastUsedMethod = 1;

-                                        }

-                                        else

-                                        {

-                                                _lastUsedMethod = 2;

-                                        }

-                                }

-

-                                bool catchDegeneratePenetrationCase =

-                                        (_catchDegeneracies != 0 && _penetrationDepthSolver != null && _degenerateSimplex != 0 && ((distance + margin) < 0.01f));

-

-                                if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))

-                                {

-#warning Check this

-                                        if (_penetrationDepthSolver != null)

-                                        {

-                                                Vector3 tmpPointOnA, tmpPointOnB;

-

-                                                _numDeepPenetrationChecks++;

-

-                                                bool isValid2 = _penetrationDepthSolver.CalculatePenetrationDepth(

-                                                        _simplexSolver, _minkowskiA, _minkowskiB, localTransA, localTransB,

-                                                        _cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB,

-                                                        debugDraw

-                                                );

-

-                                                if (isValid2)

-                                                {

-                                                        Vector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;

-                                                        float lengSqr = tmpNormalInB.LengthSquared();

-

-                                                        if (lengSqr > (MathHelper.Epsilon * MathHelper.Epsilon))

-                                                        {

-                                                                tmpNormalInB /= (float)Math.Sqrt((float)lengSqr);

-                                                                float distance2 = -(tmpPointOnA - tmpPointOnB).Length();

-

-                                                                if (!isValid || (distance2 < distance))

-                                                                {

-                                                                        distance = distance2;

-                                                                        pointOnA = tmpPointOnA;

-                                                                        pointOnB = tmpPointOnB;

-                                                                        normalInB = tmpNormalInB;

-                                                                        isValid = true;

-                                                                        _lastUsedMethod = 3;

-                                                                }

-                                                                else

-                                                                {

-

-                                                                }

-                                                        }

-                                                        else

-                                                        {

-                                                                _lastUsedMethod = 4;

-                                                        }

-                                                }

-                                                else

-                                                {

-                                                        _lastUsedMethod = 5;

-                                                }

-                                        }

-                                }

-

-                                if (isValid)

-                                {

-                                        output.AddContactPoint(normalInB, pointOnB + positionOffset, distance);

-                                }

-                        }

-                }

-        }

+/*
+  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
+  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using MonoXnaCompactMaths;
+
+namespace XnaDevRu.BulletX
+{
+        public class GjkPairDetector : DiscreteCollisionDetectorInterface
+        {
+                private Vector3 _cachedSeparatingAxis;
+                private IConvexPenetrationDepthSolver _penetrationDepthSolver;
+                private ISimplexSolver _simplexSolver;
+                private ConvexShape _minkowskiA, _minkowskiB;
+                private bool _ignoreMargin;
+
+                private int _lastUsedMethod;
+                private int _currentIteration;
+                private int _degenerateSimplex;
+                private int _catchDegeneracies;
+
+                private static int _numDeepPenetrationChecks = 0;
+                private static int _numGjkChecks = 0;
+
+                private const float RelativeError2 = 1.0e-6f;
+
+                #region Properties
+                public int LastUsedMethod
+                {
+                        get { return _lastUsedMethod; }
+                        set { _lastUsedMethod = value; }
+                }
+
+                public int CurrentIteration
+                {
+                        get { return _currentIteration; }
+                        set { _currentIteration = value; }
+                }
+
+                public int DegenerateSimplex
+                {
+                        get { return _degenerateSimplex; }
+                        set { _degenerateSimplex = value; }
+                }
+
+                public int CatchDegeneracies
+                {
+                        get { return _catchDegeneracies; }
+                        set { _catchDegeneracies = value; }
+                }
+
+                public static int DeepPenetrationChecksCount { get { return _numDeepPenetrationChecks; } }
+                public static int GjkChecksCount { get { return _numGjkChecks; } }
+                #endregion
+
+                public GjkPairDetector(ConvexShape objectA, ConvexShape objectB,
+                                                           ISimplexSolver simplexSolver,
+                                                           IConvexPenetrationDepthSolver penetrationDepthSolver)
+                {
+                        _cachedSeparatingAxis = new Vector3(0, 0, 1);
+
+                        _penetrationDepthSolver = penetrationDepthSolver;
+                        _simplexSolver = simplexSolver;
+                        _minkowskiA = objectA;
+                        _minkowskiB = objectB;
+                        _ignoreMargin = false;
+                        _lastUsedMethod = -1;
+                        _catchDegeneracies = 1;
+                }
+
+                public void setMinkowskiA(ConvexShape minkA)
+                {
+                        _minkowskiA = minkA;
+                }
+
+                public void setMinkowskiB(ConvexShape minkB)
+                {
+                        _minkowskiB = minkB;
+                }
+                public void setCachedSeperatingAxis(Vector3 seperatingAxis)
+                {
+                        _cachedSeparatingAxis = seperatingAxis;
+                }
+
+                public void setPenetrationDepthSolver(IConvexPenetrationDepthSolver penetrationDepthSolver)
+                {
+                        this._penetrationDepthSolver = penetrationDepthSolver;
+                }
+
+                public void setIgnoreMargin(bool ignoreMargin)
+                {
+                        this._ignoreMargin = ignoreMargin;
+                }
+
+                public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw)
+                {
+                        float distance = 0;
+
+                        Vector3 normalInB = new Vector3();
+                        Vector3 pointOnA = new Vector3(), pointOnB = new Vector3();
+
+                        Matrix localTransA = input.TransformA;
+                        Matrix localTransB = input.TransformB;
+
+                        Vector3 positionOffset = (localTransA.Translation + localTransB.Translation) * 0.5f;
+                        localTransA.Translation -= positionOffset;
+                        localTransB.Translation -= positionOffset;
+
+                        float marginA = _minkowskiA.Margin;
+                        float marginB = _minkowskiB.Margin;
+
+                        _numGjkChecks++;
+
+                        if (_ignoreMargin)
+                        {
+                                marginA = 0;
+                                marginB = 0;
+                        }
+
+                        _currentIteration = 0;
+
+                        int gjkMaxIter = 1000;
+                        _cachedSeparatingAxis = new Vector3(0, 1, 0);
+
+                        bool isValid = false;
+                        bool checkSimplex = false;
+                        bool checkPenetration = true;
+                        _degenerateSimplex = 0;
+
+                        _lastUsedMethod = -1;
+
+                        {
+                                float squaredDistance = MathHelper.Infinity;
+                                float delta = 0;
+
+                                float margin = marginA + marginB;
+
+                                _simplexSolver.Reset();
+
+                                while (true)
+                                {
+                                        Matrix transABasis = input.TransformA;
+                                        transABasis.Translation = Vector3.Zero;
+
+                                        Matrix transBBasis = input.TransformB;
+                                        transBBasis.Translation = Vector3.Zero;
+
+                                        Vector3 seperatingAxisInA = Vector3.TransformNormal(-_cachedSeparatingAxis, transABasis);
+                                        Vector3 seperatingAxisInB = Vector3.TransformNormal(_cachedSeparatingAxis, transBBasis);
+
+                                        Vector3 pInA = _minkowskiA.LocalGetSupportingVertexWithoutMargin(seperatingAxisInA);
+                                        Vector3 qInB = _minkowskiB.LocalGetSupportingVertexWithoutMargin(seperatingAxisInB);
+                                        Vector3 pWorld = MathHelper.MatrixToVector(localTransA, pInA);
+                                        Vector3 qWorld = MathHelper.MatrixToVector(localTransB, qInB);
+
+                                        Vector3 w = pWorld - qWorld;
+                                        delta = Vector3.Dot(_cachedSeparatingAxis, w);
+
+                                        if ((delta > 0.0) && (delta * delta > squaredDistance * input.MaximumDistanceSquared))
+                                        {
+                                                checkPenetration = false;
+                                                break;
+                                        }
+
+                                        if (_simplexSolver.InSimplex(w))
+                                        {
+                                                _degenerateSimplex = 1;
+                                                checkSimplex = true;
+                                                break;
+                                        }
+
+                                        float f0 = squaredDistance - delta;
+                                        float f1 = squaredDistance * RelativeError2;
+
+                                        if (f0 <= f1)
+                                        {
+                                                if (f0 <= 0.0f)
+                                                {
+                                                        _degenerateSimplex = 2;
+                                                }
+
+                                                checkSimplex = true;
+                                                break;
+                                        }
+
+                                        _simplexSolver.AddVertex(w, pWorld, qWorld);
+
+                                        if (!_simplexSolver.Closest(out _cachedSeparatingAxis))
+                                        {
+                                                _degenerateSimplex = 3;
+                                                checkSimplex = true;
+                                                break;
+                                        }
+
+                                        float previouseSquaredDistance = squaredDistance;
+                                        squaredDistance = _cachedSeparatingAxis.LengthSquared();
+
+                                        if (previouseSquaredDistance - squaredDistance <= MathHelper.Epsilon * previouseSquaredDistance)
+                                        {
+                                                _simplexSolver.BackupClosest(out _cachedSeparatingAxis);
+                                                checkSimplex = true;
+                                                break;
+                                        }
+
+                                        if (_currentIteration++ > gjkMaxIter)
+                                        {
+#if DEBUG
+                                                Console.WriteLine("GjkPairDetector maxIter exceeded: {0}", _currentIteration);
+                                                Console.WriteLine("sepAxis=({0},{1},{2}), squaredDistance = {3}, shapeTypeA={4}, shapeTypeB={5}",
+                                                        _cachedSeparatingAxis.X,
+                                                        _cachedSeparatingAxis.Y,
+                                                        _cachedSeparatingAxis.Z,
+                                                        squaredDistance,
+                                                        _minkowskiA.ShapeType,
+                                                        _minkowskiB.ShapeType
+                                                );
+#endif
+                                                break;
+                                        }
+
+                                        bool check = (!_simplexSolver.FullSimplex);
+
+                                        if (!check)
+                                        {
+                                                _simplexSolver.BackupClosest(out _cachedSeparatingAxis);
+                                                break;
+                                        }
+                                }
+
+                                if (checkSimplex)
+                                {
+                                        _simplexSolver.ComputePoints(out pointOnA, out pointOnB);
+                                        normalInB = pointOnA - pointOnB;
+                                        float lenSqr = _cachedSeparatingAxis.LengthSquared();
+
+                                        if (lenSqr < 0.0001f)
+                                        {
+                                                _degenerateSimplex = 5;
+                                        }
+
+                                        if (lenSqr > MathHelper.Epsilon * MathHelper.Epsilon)
+                                        {
+                                                float rlen = 1.0f / (float)Math.Sqrt((float)lenSqr);
+                                                normalInB *= rlen;
+                                                float s = (float)Math.Sqrt((float)squaredDistance);
+
+                                                BulletDebug.Assert(s > 0);
+                                                pointOnA -= _cachedSeparatingAxis * (marginA / s);
+                                                pointOnB += _cachedSeparatingAxis * (marginB / s);
+                                                distance = ((1 / rlen) - margin);
+
+                                                isValid = true;
+
+                                                _lastUsedMethod = 1;
+                                        }
+                                        else
+                                        {
+                                                _lastUsedMethod = 2;
+                                        }
+                                }
+
+                                bool catchDegeneratePenetrationCase =
+                                        (_catchDegeneracies != 0 && _penetrationDepthSolver != null && _degenerateSimplex != 0 && ((distance + margin) < 0.01f));
+
+                                if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
+                                {
+#warning Check this
+                                        if (_penetrationDepthSolver != null)
+                                        {
+                                                Vector3 tmpPointOnA, tmpPointOnB;
+
+                                                _numDeepPenetrationChecks++;
+
+                                                bool isValid2 = _penetrationDepthSolver.CalculatePenetrationDepth(
+                                                        _simplexSolver, _minkowskiA, _minkowskiB, localTransA, localTransB,
+                                                        _cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB,
+                                                        debugDraw
+                                                );
+
+                                                if (isValid2)
+                                                {
+                                                        Vector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
+                                                        float lengSqr = tmpNormalInB.LengthSquared();
+
+                                                        if (lengSqr > (MathHelper.Epsilon * MathHelper.Epsilon))
+                                                        {
+                                                                tmpNormalInB /= (float)Math.Sqrt((float)lengSqr);
+                                                                float distance2 = -(tmpPointOnA - tmpPointOnB).Length();
+
+                                                                if (!isValid || (distance2 < distance))
+                                                                {
+                                                                        distance = distance2;
+                                                                        pointOnA = tmpPointOnA;
+                                                                        pointOnB = tmpPointOnB;
+                                                                        normalInB = tmpNormalInB;
+                                                                        isValid = true;
+                                                                        _lastUsedMethod = 3;
+                                                                }
+                                                                else
+                                                                {
+
+                                                                }
+                                                        }
+                                                        else
+                                                        {
+                                                                _lastUsedMethod = 4;
+                                                        }
+                                                }
+                                                else
+                                                {
+                                                        _lastUsedMethod = 5;
+                                                }
+                                        }
+                                }
+
+                                if (isValid)
+                                {
+                                        output.AddContactPoint(normalInB, pointOnB + positionOffset, distance);
+                                }
+                        }
+                }
+        }
 }
\ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs
index 59fd486..08206c8 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs
@@ -1,42 +1,42 @@
-/*

-  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru

-  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

-

-  This software is provided 'as-is', without any express or implied

-  warranty.  In no event will the authors be held liable for any damages

-  arising from the use of this software.

-

-  Permission is granted to anyone to use this software for any purpose,

-  including commercial applications, and to alter it and redistribute it

-  freely, subject to the following restrictions:

-

-  1. The origin of this software must not be misrepresented; you must not

-     claim that you wrote the original software. If you use this software

-     in a product, an acknowledgment in the product documentation would be

-     appreciated but is not required.

-  2. Altered source versions must be plainly marked as such, and must not be

-     misrepresented as being the original software.

-  3. This notice may not be removed or altered from any source distribution.

-*/

-

-using System;

-using System.Collections.Generic;

-using System.Text;

-using MonoXnaCompactMaths;

-

-namespace XnaDevRu.BulletX

-{

-        /// <summary>

-        /// IConvexPenetrationDepthSolver provides an interface for penetration depth calculation.

-        /// </summary>

-        public interface IConvexPenetrationDepthSolver

-        {

-                bool CalculatePenetrationDepth(

-                        ISimplexSolver simplexSolver,

-                        ConvexShape convexA, ConvexShape convexB,

-                        Matrix transformA, Matrix transformB,

-                        Vector3 vector, out Vector3 ptrA, out Vector3 ptrB,

-                        IDebugDraw debugDraw//, StackAlloc stackAlloc

-                );

-        }

-}

+/*
+  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
+  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using MonoXnaCompactMaths;
+
+namespace XnaDevRu.BulletX
+{
+        /// <summary>
+        /// IConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
+        /// </summary>
+        public interface IConvexPenetrationDepthSolver
+        {
+                bool CalculatePenetrationDepth(
+                        ISimplexSolver simplexSolver,
+                        ConvexShape convexA, ConvexShape convexB,
+                        Matrix transformA, Matrix transformB,
+                        Vector3 vector, out Vector3 ptrA, out Vector3 ptrB,
+                        IDebugDraw debugDraw//, StackAlloc stackAlloc
+                );
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs
index 71b1eaa..f1b1807 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs
@@ -1,48 +1,48 @@
-/*

-  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru

-  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

-

-  This software is provided 'as-is', without any express or implied

-  warranty.  In no event will the authors be held liable for any damages

-  arising from the use of this software.

-

-  Permission is granted to anyone to use this software for any purpose,

-  including commercial applications, and to alter it and redistribute it

-  freely, subject to the following restrictions:

-

-  1. The origin of this software must not be misrepresented; you must not

-     claim that you wrote the original software. If you use this software

-     in a product, an acknowledgment in the product documentation would be

-     appreciated but is not required.

-  2. Altered source versions must be plainly marked as such, and must not be

-     misrepresented as being the original software.

-  3. This notice may not be removed or altered from any source distribution.

-*/

-

-using System;

-using System.Collections.Generic;

-using System.Text;

-using MonoXnaCompactMaths;

-

-namespace XnaDevRu.BulletX

-{

-        /// ISimplexSolver can incrementally calculate distance between origin and up to 4 vertices

-        /// Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on

-        /// voronoi regions or barycentric coordinates

-        public interface ISimplexSolver

-        {

-                void Reset();

-                void AddVertex(Vector3 w, Vector3 p, Vector3 q);

-                bool Closest(out Vector3 v);

-

-                int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf);

-                bool InSimplex(Vector3 w);

-                void BackupClosest(out Vector3 v);

-                void ComputePoints(out Vector3 pA, out Vector3 pB);

-

-                int NumVertices { get;}

-                bool EmptySimplex { get;}

-                float MaxVertex { get;}

-                bool FullSimplex { get;}

-        }

+/*
+  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
+  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using MonoXnaCompactMaths;
+
+namespace XnaDevRu.BulletX
+{
+        /// ISimplexSolver can incrementally calculate distance between origin and up to 4 vertices
+        /// Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on
+        /// voronoi regions or barycentric coordinates
+        public interface ISimplexSolver
+        {
+                void Reset();
+                void AddVertex(Vector3 w, Vector3 p, Vector3 q);
+                bool Closest(out Vector3 v);
+
+                int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf);
+                bool InSimplex(Vector3 w);
+                void BackupClosest(out Vector3 v);
+                void ComputePoints(out Vector3 pA, out Vector3 pB);
+
+                int NumVertices { get;}
+                bool EmptySimplex { get;}
+                float MaxVertex { get;}
+                bool FullSimplex { get;}
+        }
 }
\ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs
index 751a9df..f5bcedc 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs
@@ -1,78 +1,78 @@
-/*

-  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru

-  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

-

-  This software is provided 'as-is', without any express or implied

-  warranty.  In no event will the authors be held liable for any damages

-  arising from the use of this software.

-

-  Permission is granted to anyone to use this software for any purpose,

-  including commercial applications, and to alter it and redistribute it

-  freely, subject to the following restrictions:

-

-  1. The origin of this software must not be misrepresented; you must not

-     claim that you wrote the original software. If you use this software

-     in a product, an acknowledgment in the product documentation would be

-     appreciated but is not required.

-  2. Altered source versions must be plainly marked as such, and must not be

-     misrepresented as being the original software.

-  3. This notice may not be removed or altered from any source distribution.

-*/

-

-using System;

-using System.Collections.Generic;

-using System.Text;

-using MonoXnaCompactMaths;

-

-namespace XnaDevRu.BulletX

-{

-        public class ManifoldPoint

-        {

-                private Vector3 _localPointA;

-                private Vector3 _localPointB;

-                private Vector3 _positionWorldOnB;

-                private Vector3 _positionWorldOnA;

-                private Vector3 _normalWorldOnB;

-

-                private float _distance;

-                private float _combinedFriction;

-                private float _combinedRestitution;

-

-                private object _userPersistentData;

-

-                private int _lifeTime;//lifetime of the contactpoint in frames

-

-                public ManifoldPoint()

-                        : this(new Vector3(), new Vector3(), new Vector3(), 0f)

-                {

-                }

-

-                public ManifoldPoint(Vector3 pointA, Vector3 pointB,

-                                Vector3 normal,

-                                float distance)

-                {

-                        _localPointA = pointA;

-                        _localPointB = pointB;

-                        _normalWorldOnB = normal;

-                        _distance = distance;

-                        _positionWorldOnA = new Vector3();

-                        _positionWorldOnB = new Vector3();

-                }

-

-                public float Distance { get { return _distance; } set { _distance = value; } }

-                public int LifeTime { get { return _lifeTime; } set { _lifeTime = value; } }

-

-                public Vector3 PositionWorldOnA { get { return _positionWorldOnA; } set { _positionWorldOnA = value; } }

-                public Vector3 PositionWorldOnB { get { return _positionWorldOnB; } set { _positionWorldOnB = value; } }

-

-                public Vector3 LocalPointA { get { return _localPointA; } set { _localPointA = value; } }

-                public Vector3 LocalPointB { get { return _localPointB; } set { _localPointB = value; } }

-

-                public Vector3 NormalWorldOnB { get { return _normalWorldOnB; } }

-

-                public float CombinedFriction { get { return _combinedFriction; } set { _combinedFriction = value; } }

-                public float CombinedRestitution { get { return _combinedRestitution; } set { _combinedRestitution = value; } }

-

-                public object UserPersistentData { get { return _userPersistentData; } set { _userPersistentData = value; } }

-        }

-}

+/*
+  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
+  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using MonoXnaCompactMaths;
+
+namespace XnaDevRu.BulletX
+{
+        public class ManifoldPoint
+        {
+                private Vector3 _localPointA;
+                private Vector3 _localPointB;
+                private Vector3 _positionWorldOnB;
+                private Vector3 _positionWorldOnA;
+                private Vector3 _normalWorldOnB;
+
+                private float _distance;
+                private float _combinedFriction;
+                private float _combinedRestitution;
+
+                private object _userPersistentData;
+
+                private int _lifeTime;//lifetime of the contactpoint in frames
+
+                public ManifoldPoint()
+                        : this(new Vector3(), new Vector3(), new Vector3(), 0f)
+                {
+                }
+
+                public ManifoldPoint(Vector3 pointA, Vector3 pointB,
+                                Vector3 normal,
+                                float distance)
+                {
+                        _localPointA = pointA;
+                        _localPointB = pointB;
+                        _normalWorldOnB = normal;
+                        _distance = distance;
+                        _positionWorldOnA = new Vector3();
+                        _positionWorldOnB = new Vector3();
+                }
+
+                public float Distance { get { return _distance; } set { _distance = value; } }
+                public int LifeTime { get { return _lifeTime; } set { _lifeTime = value; } }
+
+                public Vector3 PositionWorldOnA { get { return _positionWorldOnA; } set { _positionWorldOnA = value; } }
+                public Vector3 PositionWorldOnB { get { return _positionWorldOnB; } set { _positionWorldOnB = value; } }
+
+                public Vector3 LocalPointA { get { return _localPointA; } set { _localPointA = value; } }
+                public Vector3 LocalPointB { get { return _localPointB; } set { _localPointB = value; } }
+
+                public Vector3 NormalWorldOnB { get { return _normalWorldOnB; } }
+
+                public float CombinedFriction { get { return _combinedFriction; } set { _combinedFriction = value; } }
+                public float CombinedRestitution { get { return _combinedRestitution; } set { _combinedRestitution = value; } }
+
+                public object UserPersistentData { get { return _userPersistentData; } set { _userPersistentData = value; } }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs
index 81b05b8..7363e28 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs
@@ -1,246 +1,246 @@
-/*

-  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru

-  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

-

-  This software is provided 'as-is', without any express or implied

-  warranty.  In no event will the authors be held liable for any damages

-  arising from the use of this software.

-

-  Permission is granted to anyone to use this software for any purpose,

-  including commercial applications, and to alter it and redistribute it

-  freely, subject to the following restrictions:

-

-  1. The origin of this software must not be misrepresented; you must not

-     claim that you wrote the original software. If you use this software

-     in a product, an acknowledgment in the product documentation would be

-     appreciated but is not required.

-  2. Altered source versions must be plainly marked as such, and must not be

-     misrepresented as being the original software.

-  3. This notice may not be removed or altered from any source distribution.

-*/

-

-using System;

-using System.Collections.Generic;

-using System.Text;

-using MonoXnaCompactMaths;

-

-namespace XnaDevRu.BulletX

-{

-        /// <summary>

-        /// MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation.

-        /// Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points.

-        /// </summary>

-        public class MinkowskiPenetrationDepthSolver : IConvexPenetrationDepthSolver

-        {

-                private const int UnitSpherePointsCount = 42;

-

-                private static Vector3[] penetrationDirections = 

-                                                                                                                {

-                                                                                                                new Vector3(0.000000f , -0.000000f,-1.000000f),

-                                                                                                                new Vector3(0.723608f , -0.525725f,-0.447219f),

-                                                                                                                new Vector3(-0.276388f , -0.850649f,-0.447219f),

-                                                                                                                new Vector3(-0.894426f , -0.000000f,-0.447216f),

-                                                                                                                new Vector3(-0.276388f , 0.850649f,-0.447220f),

-                                                                                                                new Vector3(0.723608f , 0.525725f,-0.447219f),

-                                                                                                                new Vector3(0.276388f , -0.850649f,0.447220f),

-                                                                                                                new Vector3(-0.723608f , -0.525725f,0.447219f),

-                                                                                                                new Vector3(-0.723608f , 0.525725f,0.447219f),

-                                                                                                                new Vector3(0.276388f , 0.850649f,0.447219f),

-                                                                                                                new Vector3(0.894426f , 0.000000f,0.447216f),

-                                                                                                                new Vector3(-0.000000f , 0.000000f,1.000000f),

-                                                                                                                new Vector3(0.425323f , -0.309011f,-0.850654f),

-                                                                                                                new Vector3(-0.162456f , -0.499995f,-0.850654f),

-                                                                                                                new Vector3(0.262869f , -0.809012f,-0.525738f),

-                                                                                                                new Vector3(0.425323f , 0.309011f,-0.850654f),

-                                                                                                                new Vector3(0.850648f , -0.000000f,-0.525736f),

-                                                                                                                new Vector3(-0.525730f , -0.000000f,-0.850652f),

-                                                                                                                new Vector3(-0.688190f , -0.499997f,-0.525736f),

-                                                                                                                new Vector3(-0.162456f , 0.499995f,-0.850654f),

-                                                                                                                new Vector3(-0.688190f , 0.499997f,-0.525736f),

-                                                                                                                new Vector3(0.262869f , 0.809012f,-0.525738f),

-                                                                                                                new Vector3(0.951058f , 0.309013f,0.000000f),

-                                                                                                                new Vector3(0.951058f , -0.309013f,0.000000f),

-                                                                                                                new Vector3(0.587786f , -0.809017f,0.000000f),

-                                                                                                                new Vector3(0.000000f , -1.000000f,0.000000f),

-                                                                                                                new Vector3(-0.587786f , -0.809017f,0.000000f),

-                                                                                                                new Vector3(-0.951058f , -0.309013f,-0.000000f),

-                                                                                                                new Vector3(-0.951058f , 0.309013f,-0.000000f),

-                                                                                                                new Vector3(-0.587786f , 0.809017f,-0.000000f),

-                                                                                                                new Vector3(-0.000000f , 1.000000f,-0.000000f),

-                                                                                                                new Vector3(0.587786f , 0.809017f,-0.000000f),

-                                                                                                                new Vector3(0.688190f , -0.499997f,0.525736f),

-                                                                                                                new Vector3(-0.262869f , -0.809012f,0.525738f),

-                                                                                                                new Vector3(-0.850648f , 0.000000f,0.525736f),

-                                                                                                                new Vector3(-0.262869f , 0.809012f,0.525738f),

-                                                                                                                new Vector3(0.688190f , 0.499997f,0.525736f),

-                                                                                                                new Vector3(0.525730f , 0.000000f,0.850652f),

-                                                                                                                new Vector3(0.162456f , -0.499995f,0.850654f),

-                                                                                                                new Vector3(-0.425323f , -0.309011f,0.850654f),

-                                                                                                                new Vector3(-0.425323f , 0.309011f,0.850654f),

-                                                                                                                new Vector3(0.162456f , 0.499995f,0.850654f)

-                                                                                                                };

-

-                private class IntermediateResult : DiscreteCollisionDetectorInterface.Result

-                {

-                        private Vector3 _normalOnBInWorld;

-                        private Vector3 _pointInWorld;

-                        private float _depth;

-                        private bool _hasResult;

-

-                        public IntermediateResult()

-                        {

-                                _hasResult = false;

-                        }

-

-                        public bool HasResult { get { return _hasResult; } }

-                        public float Depth { get { return _depth; } }

-                        public Vector3 PointInWorld { get { return _pointInWorld; } }

-

-                        public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1)

-                        {

-                        }

-

-                        public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth)

-                        {

-                                _normalOnBInWorld = normalOnBInWorld;

-                                _pointInWorld = pointInWorld;

-                                _depth = depth;

-                                _hasResult = true;

-                        }

-                }

-

-                #region IConvexPenetrationDepthSolver Members

-                public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver,

-                        ConvexShape convexA, ConvexShape convexB,

-                        Matrix transformA, Matrix transformB,

-                        Vector3 v, out Vector3 pa, out Vector3 pb, IDebugDraw debugDraw)

-                {

-                        pa = new Vector3();

-                        pb = new Vector3();

-                        //just take fixed number of orientation, and sample the penetration depth in that direction

-                        float minProj = 1e30f;

-                        Vector3 minNorm = new Vector3();

-                        Vector3 minA = new Vector3(), minB = new Vector3();

-                        Vector3 seperatingAxisInA, seperatingAxisInB;

-                        Vector3 pInA, qInB, pWorld, qWorld, w;

-

-                        Vector3[] supportVerticesABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];

-                        Vector3[] supportVerticesBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];

-                        Vector3[] seperatingAxisInABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];

-                        Vector3[] seperatingAxisInBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];

-

-                        int numSampleDirections = UnitSpherePointsCount;

-

-                        for (int i = 0; i < numSampleDirections; i++)

-                        {

-                                Vector3 norm = penetrationDirections[i];

-                                seperatingAxisInABatch[i] = Vector3.TransformNormal((-norm), transformA);

-                                seperatingAxisInBBatch[i] = Vector3.TransformNormal(norm, transformB);

-                        }

-

-                        {

-                                int numPDA = convexA.PreferredPenetrationDirectionsCount;

-                                if (numPDA != 0)

-                                {

-                                        for (int i = 0; i < numPDA; i++)

-                                        {

-                                                Vector3 norm;

-                                                convexA.GetPreferredPenetrationDirection(i, out norm);

-                                                norm = Vector3.TransformNormal(norm, transformA);

-                                                penetrationDirections[numSampleDirections] = norm;

-                                                seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA);

-                                                seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB);

-                                                numSampleDirections++;

-                                        }

-                                }

-                        }

-

-                        {

-                                int numPDB = convexB.PreferredPenetrationDirectionsCount;

-                                if (numPDB != 0)

-                                {

-                                        for (int i = 0; i < numPDB; i++)

-                                        {

-                                                Vector3 norm;

-                                                convexB.GetPreferredPenetrationDirection(i, out norm);

-                                                norm = Vector3.TransformNormal(norm, transformB);

-                                                penetrationDirections[numSampleDirections] = norm;

-                                                seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA);

-                                                seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB);

-                                                numSampleDirections++;

-                                        }

-                                }

-                        }

-

-                        convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch); //, numSampleDirections);

-                        convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch); //, numSampleDirections);

-

-                        for (int i = 0; i < numSampleDirections; i++)

-                        {

-                                Vector3 norm = penetrationDirections[i];

-                                seperatingAxisInA = seperatingAxisInABatch[i];

-                                seperatingAxisInB = seperatingAxisInBBatch[i];

-

-                                pInA = supportVerticesABatch[i];

-                                qInB = supportVerticesBBatch[i];

-

-                                pWorld = MathHelper.MatrixToVector(transformA, pInA);

-                                qWorld = MathHelper.MatrixToVector(transformB, qInB);

-                                w = qWorld - pWorld;

-                                float delta = Vector3.Dot(norm, w);

-                                //find smallest delta

-                                if (delta < minProj)

-                                {

-                                        minProj = delta;

-                                        minNorm = norm;

-                                        minA = pWorld;

-                                        minB = qWorld;

-                                }

-                        }

-

-                        //add the margins

-                        minA += minNorm * convexA.Margin;

-                        minB -= minNorm * convexB.Margin;

-                        //no penetration

-                        if (minProj < 0)

-                                return false;

-

-                        minProj += (convexA.Margin + convexB.Margin);

-

-                        GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null);

-

-                        float offsetDist = minProj;

-                        Vector3 offset = minNorm * offsetDist;

-

-                        GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();

-

-                        Vector3 newOrg = transformA.Translation + offset;

-

-                        Matrix displacedTrans = transformA;

-                        displacedTrans.Translation = newOrg;

-

-                        input.TransformA = displacedTrans;

-                        input.TransformB = transformB;

-                        input.MaximumDistanceSquared = 1e30f;//minProj;

-

-                        IntermediateResult res = new IntermediateResult();

-                        gjkdet.GetClosestPoints(input, res, debugDraw);

-

-                        float correctedMinNorm = minProj - res.Depth;

-

-                        //the penetration depth is over-estimated, relax it

-                        float penetration_relaxation = 1;

-                        minNorm *= penetration_relaxation;

-

-                        if (res.HasResult)

-                        {

-

-                                pa = res.PointInWorld - minNorm * correctedMinNorm;

-                                pb = res.PointInWorld;

-                        }

-

-                        return res.HasResult;

-                }

-                #endregion

-        }

-}

+/*
+  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
+  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using MonoXnaCompactMaths;
+
+namespace XnaDevRu.BulletX
+{
+        /// <summary>
+        /// MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation.
+        /// Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points.
+        /// </summary>
+        public class MinkowskiPenetrationDepthSolver : IConvexPenetrationDepthSolver
+        {
+                private const int UnitSpherePointsCount = 42;
+
+                private static Vector3[] penetrationDirections = 
+                                                                                                                {
+                                                                                                                new Vector3(0.000000f , -0.000000f,-1.000000f),
+                                                                                                                new Vector3(0.723608f , -0.525725f,-0.447219f),
+                                                                                                                new Vector3(-0.276388f , -0.850649f,-0.447219f),
+                                                                                                                new Vector3(-0.894426f , -0.000000f,-0.447216f),
+                                                                                                                new Vector3(-0.276388f , 0.850649f,-0.447220f),
+                                                                                                                new Vector3(0.723608f , 0.525725f,-0.447219f),
+                                                                                                                new Vector3(0.276388f , -0.850649f,0.447220f),
+                                                                                                                new Vector3(-0.723608f , -0.525725f,0.447219f),
+                                                                                                                new Vector3(-0.723608f , 0.525725f,0.447219f),
+                                                                                                                new Vector3(0.276388f , 0.850649f,0.447219f),
+                                                                                                                new Vector3(0.894426f , 0.000000f,0.447216f),
+                                                                                                                new Vector3(-0.000000f , 0.000000f,1.000000f),
+                                                                                                                new Vector3(0.425323f , -0.309011f,-0.850654f),
+                                                                                                                new Vector3(-0.162456f , -0.499995f,-0.850654f),
+                                                                                                                new Vector3(0.262869f , -0.809012f,-0.525738f),
+                                                                                                                new Vector3(0.425323f , 0.309011f,-0.850654f),
+                                                                                                                new Vector3(0.850648f , -0.000000f,-0.525736f),
+                                                                                                                new Vector3(-0.525730f , -0.000000f,-0.850652f),
+                                                                                                                new Vector3(-0.688190f , -0.499997f,-0.525736f),
+                                                                                                                new Vector3(-0.162456f , 0.499995f,-0.850654f),
+                                                                                                                new Vector3(-0.688190f , 0.499997f,-0.525736f),
+                                                                                                                new Vector3(0.262869f , 0.809012f,-0.525738f),
+                                                                                                                new Vector3(0.951058f , 0.309013f,0.000000f),
+                                                                                                                new Vector3(0.951058f , -0.309013f,0.000000f),
+                                                                                                                new Vector3(0.587786f , -0.809017f,0.000000f),
+                                                                                                                new Vector3(0.000000f , -1.000000f,0.000000f),
+                                                                                                                new Vector3(-0.587786f , -0.809017f,0.000000f),
+                                                                                                                new Vector3(-0.951058f , -0.309013f,-0.000000f),
+                                                                                                                new Vector3(-0.951058f , 0.309013f,-0.000000f),
+                                                                                                                new Vector3(-0.587786f , 0.809017f,-0.000000f),
+                                                                                                                new Vector3(-0.000000f , 1.000000f,-0.000000f),
+                                                                                                                new Vector3(0.587786f , 0.809017f,-0.000000f),
+                                                                                                                new Vector3(0.688190f , -0.499997f,0.525736f),
+                                                                                                                new Vector3(-0.262869f , -0.809012f,0.525738f),
+                                                                                                                new Vector3(-0.850648f , 0.000000f,0.525736f),
+                                                                                                                new Vector3(-0.262869f , 0.809012f,0.525738f),
+                                                                                                                new Vector3(0.688190f , 0.499997f,0.525736f),
+                                                                                                                new Vector3(0.525730f , 0.000000f,0.850652f),
+                                                                                                                new Vector3(0.162456f , -0.499995f,0.850654f),
+                                                                                                                new Vector3(-0.425323f , -0.309011f,0.850654f),
+                                                                                                                new Vector3(-0.425323f , 0.309011f,0.850654f),
+                                                                                                                new Vector3(0.162456f , 0.499995f,0.850654f)
+                                                                                                                };
+
+                private class IntermediateResult : DiscreteCollisionDetectorInterface.Result
+                {
+                        private Vector3 _normalOnBInWorld;
+                        private Vector3 _pointInWorld;
+                        private float _depth;
+                        private bool _hasResult;
+
+                        public IntermediateResult()
+                        {
+                                _hasResult = false;
+                        }
+
+                        public bool HasResult { get { return _hasResult; } }
+                        public float Depth { get { return _depth; } }
+                        public Vector3 PointInWorld { get { return _pointInWorld; } }
+
+                        public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1)
+                        {
+                        }
+
+                        public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth)
+                        {
+                                _normalOnBInWorld = normalOnBInWorld;
+                                _pointInWorld = pointInWorld;
+                                _depth = depth;
+                                _hasResult = true;
+                        }
+                }
+
+                #region IConvexPenetrationDepthSolver Members
+                public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver,
+                        ConvexShape convexA, ConvexShape convexB,
+                        Matrix transformA, Matrix transformB,
+                        Vector3 v, out Vector3 pa, out Vector3 pb, IDebugDraw debugDraw)
+                {
+                        pa = new Vector3();
+                        pb = new Vector3();
+                        //just take fixed number of orientation, and sample the penetration depth in that direction
+                        float minProj = 1e30f;
+                        Vector3 minNorm = new Vector3();
+                        Vector3 minA = new Vector3(), minB = new Vector3();
+                        Vector3 seperatingAxisInA, seperatingAxisInB;
+                        Vector3 pInA, qInB, pWorld, qWorld, w;
+
+                        Vector3[] supportVerticesABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
+                        Vector3[] supportVerticesBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
+                        Vector3[] seperatingAxisInABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
+                        Vector3[] seperatingAxisInBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
+
+                        int numSampleDirections = UnitSpherePointsCount;
+
+                        for (int i = 0; i < numSampleDirections; i++)
+                        {
+                                Vector3 norm = penetrationDirections[i];
+                                seperatingAxisInABatch[i] = Vector3.TransformNormal((-norm), transformA);
+                                seperatingAxisInBBatch[i] = Vector3.TransformNormal(norm, transformB);
+                        }
+
+                        {
+                                int numPDA = convexA.PreferredPenetrationDirectionsCount;
+                                if (numPDA != 0)
+                                {
+                                        for (int i = 0; i < numPDA; i++)
+                                        {
+                                                Vector3 norm;
+                                                convexA.GetPreferredPenetrationDirection(i, out norm);
+                                                norm = Vector3.TransformNormal(norm, transformA);
+                                                penetrationDirections[numSampleDirections] = norm;
+                                                seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA);
+                                                seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB);
+                                                numSampleDirections++;
+                                        }
+                                }
+                        }
+
+                        {
+                                int numPDB = convexB.PreferredPenetrationDirectionsCount;
+                                if (numPDB != 0)
+                                {
+                                        for (int i = 0; i < numPDB; i++)
+                                        {
+                                                Vector3 norm;
+                                                convexB.GetPreferredPenetrationDirection(i, out norm);
+                                                norm = Vector3.TransformNormal(norm, transformB);
+                                                penetrationDirections[numSampleDirections] = norm;
+                                                seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA);
+                                                seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB);
+                                                numSampleDirections++;
+                                        }
+                                }
+                        }
+
+                        convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch); //, numSampleDirections);
+                        convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch); //, numSampleDirections);
+
+                        for (int i = 0; i < numSampleDirections; i++)
+                        {
+                                Vector3 norm = penetrationDirections[i];
+                                seperatingAxisInA = seperatingAxisInABatch[i];
+                                seperatingAxisInB = seperatingAxisInBBatch[i];
+
+                                pInA = supportVerticesABatch[i];
+                                qInB = supportVerticesBBatch[i];
+
+                                pWorld = MathHelper.MatrixToVector(transformA, pInA);
+                                qWorld = MathHelper.MatrixToVector(transformB, qInB);
+                                w = qWorld - pWorld;
+                                float delta = Vector3.Dot(norm, w);
+                                //find smallest delta
+                                if (delta < minProj)
+                                {
+                                        minProj = delta;
+                                        minNorm = norm;
+                                        minA = pWorld;
+                                        minB = qWorld;
+                                }
+                        }
+
+                        //add the margins
+                        minA += minNorm * convexA.Margin;
+                        minB -= minNorm * convexB.Margin;
+                        //no penetration
+                        if (minProj < 0)
+                                return false;
+
+                        minProj += (convexA.Margin + convexB.Margin);
+
+                        GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null);
+
+                        float offsetDist = minProj;
+                        Vector3 offset = minNorm * offsetDist;
+
+                        GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
+
+                        Vector3 newOrg = transformA.Translation + offset;
+
+                        Matrix displacedTrans = transformA;
+                        displacedTrans.Translation = newOrg;
+
+                        input.TransformA = displacedTrans;
+                        input.TransformB = transformB;
+                        input.MaximumDistanceSquared = 1e30f;//minProj;
+
+                        IntermediateResult res = new IntermediateResult();
+                        gjkdet.GetClosestPoints(input, res, debugDraw);
+
+                        float correctedMinNorm = minProj - res.Depth;
+
+                        //the penetration depth is over-estimated, relax it
+                        float penetration_relaxation = 1;
+                        minNorm *= penetration_relaxation;
+
+                        if (res.HasResult)
+                        {
+
+                                pa = res.PointInWorld - minNorm * correctedMinNorm;
+                                pb = res.PointInWorld;
+                        }
+
+                        return res.HasResult;
+                }
+                #endregion
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs
index 47cfe9a..04a533d 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs
@@ -1,272 +1,272 @@
-/*

-  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru

-  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

-

-  This software is provided 'as-is', without any express or implied

-  warranty.  In no event will the authors be held liable for any damages

-  arising from the use of this software.

-

-  Permission is granted to anyone to use this software for any purpose,

-  including commercial applications, and to alter it and redistribute it

-  freely, subject to the following restrictions:

-

-  1. The origin of this software must not be misrepresented; you must not

-     claim that you wrote the original software. If you use this software

-     in a product, an acknowledgment in the product documentation would be

-     appreciated but is not required.

-  2. Altered source versions must be plainly marked as such, and must not be

-     misrepresented as being the original software.

-  3. This notice may not be removed or altered from any source distribution.

-*/

-

-using System;

-using System.Collections.Generic;

-using System.Text;

-using MonoXnaCompactMaths;

-

-namespace XnaDevRu.BulletX

-{

-        public delegate bool ContactDestroyedCallback(object userPersistentData);

-

-        public class PersistentManifold

-        {

-                private static ContactDestroyedCallback _contactDestroyedCallback = null;

-                private static float _contactBreakingThreshold = 0.02f;

-

-                private ManifoldPoint[] _pointCache = new ManifoldPoint[4];

-

-                // this two body pointers can point to the physics rigidbody class.

-                // object will allow any rigidbody class

-                private object _bodyA;

-                private object _bodyB;

-                private int _cachedPoints;

-

-                public PersistentManifold(object bodyA, object bodyB)

-                {

-                        _bodyA = bodyA;

-                        _bodyB = bodyB;

-                        _cachedPoints = 0;

-                }

-

-                public object BodyA { get { return _bodyA; } }

-                public object BodyB { get { return _bodyB; } }

-

-                public int ContactsCount { get { return _cachedPoints; } }

-

-                public static ContactDestroyedCallback ContactDestroyedCallback { get { return _contactDestroyedCallback; } set { _contactDestroyedCallback = value; } }

-                public static float ContactBreakingThreshold { get { return _contactBreakingThreshold; } }

-

-                public void SetBodies(object bodyA, object bodyB)

-                {

-                        _bodyA = bodyA;

-                        _bodyB = bodyB;

-                }

-

-                public ManifoldPoint GetContactPoint(int index)

-                {

-                        if (index >= _cachedPoints)

-                                throw new ArgumentOutOfRangeException("index", "index must be smaller than cachedPoints");

-

-                        return _pointCache[index];

-                }

-

-                public int GetCacheEntry(ManifoldPoint newPoint)

-                {

-                        float shortestDist = ContactBreakingThreshold * ContactBreakingThreshold;

-                        int size = ContactsCount;

-                        int nearestPoint = -1;

-                        for (int i = 0; i < size; i++)

-                        {

-                                ManifoldPoint mp = _pointCache[i];

-

-                                Vector3 diffA = mp.LocalPointA - newPoint.LocalPointA;

-                                float distToManiPoint = Vector3.Dot(diffA, diffA);

-                                if (distToManiPoint < shortestDist)

-                                {

-                                        shortestDist = distToManiPoint;

-                                        nearestPoint = i;

-                                }

-                        }

-                        return nearestPoint;

-                }

-

-                public void AddManifoldPoint(ManifoldPoint newPoint)

-                {

-                        if (!ValidContactDistance(newPoint))

-                                throw new BulletException();

-

-                        int insertIndex = ContactsCount;

-                        if (insertIndex == 4)

-                        {

-                                //sort cache so best points come first, based on area

-                                insertIndex = SortCachedPoints(newPoint);

-                        }

-                        else

-                        {

-                                _cachedPoints++;

-                        }

-                        ReplaceContactPoint(newPoint, insertIndex);

-                }

-

-                public void RemoveContactPoint(int index)

-                {

-                        ClearUserCache(_pointCache[index]);

-

-                        int lastUsedIndex = ContactsCount - 1;

-                        _pointCache[index] = _pointCache[lastUsedIndex];

-                        //get rid of duplicated userPersistentData pointer

-                        _pointCache[lastUsedIndex].UserPersistentData = null;

-                        _cachedPoints--;

-                }

-

-                public void ReplaceContactPoint(ManifoldPoint newPoint, int insertIndex)

-                {

-                        BulletDebug.Assert(ValidContactDistance(newPoint));

-

-                        if (_pointCache[insertIndex] != null)

-                        {

-                                int lifeTime = _pointCache[insertIndex].LifeTime;

-                                BulletDebug.Assert(lifeTime >= 0);

-                                object cache = _pointCache[insertIndex].UserPersistentData;

-

-                                _pointCache[insertIndex] = newPoint;

-

-                                _pointCache[insertIndex].UserPersistentData = cache;

-                                _pointCache[insertIndex].LifeTime = lifeTime;

-                        }

-                        else

-                        {

-                                _pointCache[insertIndex] = newPoint;

-                        }

-

-                        //ClearUserCache(_pointCache[insertIndex]);

-                        //_pointCache[insertIndex] = newPoint;

-                }

-

-                public bool ValidContactDistance(ManifoldPoint pt)

-                {

-                        return pt.Distance <= ContactBreakingThreshold;

-                }

-

-                // calculated new worldspace coordinates and depth, and reject points that exceed the collision margin

-                public void RefreshContactPoints(Matrix trA, Matrix trB)

-                {

-                        // first refresh worldspace positions and distance

-                        for (int i = ContactsCount - 1; i >= 0; i--)

-                        {

-                                ManifoldPoint manifoldPoint = _pointCache[i];

-                                manifoldPoint.PositionWorldOnA = MathHelper.MatrixToVector(trA,manifoldPoint.LocalPointA);

-                                manifoldPoint.PositionWorldOnB = MathHelper.MatrixToVector(trB, manifoldPoint.LocalPointB);

-                                manifoldPoint.Distance = Vector3.Dot(manifoldPoint.PositionWorldOnA - manifoldPoint.PositionWorldOnB, manifoldPoint.NormalWorldOnB);

-                                manifoldPoint.LifeTime++;

-                        }

-

-                        // then 

-                        float distance2d;

-                        Vector3 projectedDifference, projectedPoint;

-                        for (int i = ContactsCount - 1; i >= 0; i--)

-                        {

-

-                                ManifoldPoint manifoldPoint = _pointCache[i];

-                                //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)

-                                if (!ValidContactDistance(manifoldPoint))

-                                {

-                                        RemoveContactPoint(i);

-                                }

-                                else

-                                {

-                                        //contact also becomes invalid when relative movement orthogonal to normal exceeds margin

-                                        projectedPoint = manifoldPoint.PositionWorldOnA - manifoldPoint.NormalWorldOnB * manifoldPoint.Distance;

-                                        projectedDifference = manifoldPoint.PositionWorldOnB - projectedPoint;

-                                        distance2d = Vector3.Dot(projectedDifference, projectedDifference);

-                                        if (distance2d > ContactBreakingThreshold * ContactBreakingThreshold)

-                                        {

-                                                RemoveContactPoint(i);

-                                        }

-                                }

-                        }

-                }

-

-                public void ClearManifold()

-                {

-                        for (int i = 0; i < _cachedPoints; i++)

-                        {

-                                ClearUserCache(_pointCache[i]);

-                        }

-                        _cachedPoints = 0;

-                }

-

-                private void ClearUserCache(ManifoldPoint pt)

-                {

-                        if (pt != null)

-                        {

-                                object oldPtr = pt.UserPersistentData;

-

-                                if (oldPtr != null)

-                                {

-                                        if (pt.UserPersistentData != null && _contactDestroyedCallback != null)

-                                        {

-                                                _contactDestroyedCallback(pt.UserPersistentData);

-                                                pt.UserPersistentData = null;

-                                        }

-                                }

-                        }

-                }

-

-                // sort cached points so most isolated points come first

-                private int SortCachedPoints(ManifoldPoint pt)

-                {

-                        //calculate 4 possible cases areas, and take biggest area

-                        //also need to keep 'deepest'

-

-                        int maxPenetrationIndex = -1;

-                        float maxPenetration = pt.Distance;

-                        for (int i = 0; i < 4; i++)

-                        {

-                                if (_pointCache[i].Distance < maxPenetration)

-                                {

-                                        maxPenetrationIndex = i;

-                                        maxPenetration = _pointCache[i].Distance;

-                                }

-                        }

-

-                        float res0 = 0, res1 = 0, res2 = 0, res3 = 0;

-                        if (maxPenetrationIndex != 0)

-                        {

-                                Vector3 a0 = pt.LocalPointA - _pointCache[1].LocalPointA;

-                                Vector3 b0 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA;

-                                Vector3 cross = Vector3.Cross(a0, b0);

-                                res0 = cross.LengthSquared();

-                        }

-                        if (maxPenetrationIndex != 1)

-                        {

-                                Vector3 a1 = pt.LocalPointA - _pointCache[0].LocalPointA;

-                                Vector3 b1 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA;

-                                Vector3 cross = Vector3.Cross(a1, b1);

-                                res1 = cross.LengthSquared();

-                        }

-

-                        if (maxPenetrationIndex != 2)

-                        {

-                                Vector3 a2 = pt.LocalPointA - _pointCache[0].LocalPointA;

-                                Vector3 b2 = _pointCache[3].LocalPointA - _pointCache[1].LocalPointA;

-                                Vector3 cross = Vector3.Cross(a2, b2);

-                                res2 = cross.LengthSquared();

-                        }

-

-                        if (maxPenetrationIndex != 3)

-                        {

-                                Vector3 a3 = pt.LocalPointA - _pointCache[0].LocalPointA;

-                                Vector3 b3 = _pointCache[2].LocalPointA - _pointCache[1].LocalPointA;

-                                Vector3 cross = Vector3.Cross(a3, b3);

-                                res3 = cross.LengthSquared();

-                        }

-

-                        Vector4 maxvec = new Vector4(res0, res1, res2, res3);

-                        int biggestarea = MathHelper.ClosestAxis(maxvec);

-                        return biggestarea;

-                }

-

-                private int FindContactPoint(ManifoldPoint unUsed, int numUnused, ManifoldPoint pt) { return 0; }

-        }

-}

+/*
+  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
+  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using MonoXnaCompactMaths;
+
+namespace XnaDevRu.BulletX
+{
+        public delegate bool ContactDestroyedCallback(object userPersistentData);
+
+        public class PersistentManifold
+        {
+                private static ContactDestroyedCallback _contactDestroyedCallback = null;
+                private static float _contactBreakingThreshold = 0.02f;
+
+                private ManifoldPoint[] _pointCache = new ManifoldPoint[4];
+
+                // this two body pointers can point to the physics rigidbody class.
+                // object will allow any rigidbody class
+                private object _bodyA;
+                private object _bodyB;
+                private int _cachedPoints;
+
+                public PersistentManifold(object bodyA, object bodyB)
+                {
+                        _bodyA = bodyA;
+                        _bodyB = bodyB;
+                        _cachedPoints = 0;
+                }
+
+                public object BodyA { get { return _bodyA; } }
+                public object BodyB { get { return _bodyB; } }
+
+                public int ContactsCount { get { return _cachedPoints; } }
+
+                public static ContactDestroyedCallback ContactDestroyedCallback { get { return _contactDestroyedCallback; } set { _contactDestroyedCallback = value; } }
+                public static float ContactBreakingThreshold { get { return _contactBreakingThreshold; } }
+
+                public void SetBodies(object bodyA, object bodyB)
+                {
+                        _bodyA = bodyA;
+                        _bodyB = bodyB;
+                }
+
+                public ManifoldPoint GetContactPoint(int index)
+                {
+                        if (index >= _cachedPoints)
+                                throw new ArgumentOutOfRangeException("index", "index must be smaller than cachedPoints");
+
+                        return _pointCache[index];
+                }
+
+                public int GetCacheEntry(ManifoldPoint newPoint)
+                {
+                        float shortestDist = ContactBreakingThreshold * ContactBreakingThreshold;
+                        int size = ContactsCount;
+                        int nearestPoint = -1;
+                        for (int i = 0; i < size; i++)
+                        {
+                                ManifoldPoint mp = _pointCache[i];
+
+                                Vector3 diffA = mp.LocalPointA - newPoint.LocalPointA;
+                                float distToManiPoint = Vector3.Dot(diffA, diffA);
+                                if (distToManiPoint < shortestDist)
+                                {
+                                        shortestDist = distToManiPoint;
+                                        nearestPoint = i;
+                                }
+                        }
+                        return nearestPoint;
+                }
+
+                public void AddManifoldPoint(ManifoldPoint newPoint)
+                {
+                        if (!ValidContactDistance(newPoint))
+                                throw new BulletException();
+
+                        int insertIndex = ContactsCount;
+                        if (insertIndex == 4)
+                        {
+                                //sort cache so best points come first, based on area
+                                insertIndex = SortCachedPoints(newPoint);
+                        }
+                        else
+                        {
+                                _cachedPoints++;
+                        }
+                        ReplaceContactPoint(newPoint, insertIndex);
+                }
+
+                public void RemoveContactPoint(int index)
+                {
+                        ClearUserCache(_pointCache[index]);
+
+                        int lastUsedIndex = ContactsCount - 1;
+                        _pointCache[index] = _pointCache[lastUsedIndex];
+                        //get rid of duplicated userPersistentData pointer
+                        _pointCache[lastUsedIndex].UserPersistentData = null;
+                        _cachedPoints--;
+                }
+
+                public void ReplaceContactPoint(ManifoldPoint newPoint, int insertIndex)
+                {
+                        BulletDebug.Assert(ValidContactDistance(newPoint));
+
+                        if (_pointCache[insertIndex] != null)
+                        {
+                                int lifeTime = _pointCache[insertIndex].LifeTime;
+                                BulletDebug.Assert(lifeTime >= 0);
+                                object cache = _pointCache[insertIndex].UserPersistentData;
+
+                                _pointCache[insertIndex] = newPoint;
+
+                                _pointCache[insertIndex].UserPersistentData = cache;
+                                _pointCache[insertIndex].LifeTime = lifeTime;
+                        }
+                        else
+                        {
+                                _pointCache[insertIndex] = newPoint;
+                        }
+
+                        //ClearUserCache(_pointCache[insertIndex]);
+                        //_pointCache[insertIndex] = newPoint;
+                }
+
+                public bool ValidContactDistance(ManifoldPoint pt)
+                {
+                        return pt.Distance <= ContactBreakingThreshold;
+                }
+
+                // calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
+                public void RefreshContactPoints(Matrix trA, Matrix trB)
+                {
+                        // first refresh worldspace positions and distance
+                        for (int i = ContactsCount - 1; i >= 0; i--)
+                        {
+                                ManifoldPoint manifoldPoint = _pointCache[i];
+                                manifoldPoint.PositionWorldOnA = MathHelper.MatrixToVector(trA,manifoldPoint.LocalPointA);
+                                manifoldPoint.PositionWorldOnB = MathHelper.MatrixToVector(trB, manifoldPoint.LocalPointB);
+                                manifoldPoint.Distance = Vector3.Dot(manifoldPoint.PositionWorldOnA - manifoldPoint.PositionWorldOnB, manifoldPoint.NormalWorldOnB);
+                                manifoldPoint.LifeTime++;
+                        }
+
+                        // then 
+                        float distance2d;
+                        Vector3 projectedDifference, projectedPoint;
+                        for (int i = ContactsCount - 1; i >= 0; i--)
+                        {
+
+                                ManifoldPoint manifoldPoint = _pointCache[i];
+                                //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
+                                if (!ValidContactDistance(manifoldPoint))
+                                {
+                                        RemoveContactPoint(i);
+                                }
+                                else
+                                {
+                                        //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
+                                        projectedPoint = manifoldPoint.PositionWorldOnA - manifoldPoint.NormalWorldOnB * manifoldPoint.Distance;
+                                        projectedDifference = manifoldPoint.PositionWorldOnB - projectedPoint;
+                                        distance2d = Vector3.Dot(projectedDifference, projectedDifference);
+                                        if (distance2d > ContactBreakingThreshold * ContactBreakingThreshold)
+                                        {
+                                                RemoveContactPoint(i);
+                                        }
+                                }
+                        }
+                }
+
+                public void ClearManifold()
+                {
+                        for (int i = 0; i < _cachedPoints; i++)
+                        {
+                                ClearUserCache(_pointCache[i]);
+                        }
+                        _cachedPoints = 0;
+                }
+
+                private void ClearUserCache(ManifoldPoint pt)
+                {
+                        if (pt != null)
+                        {
+                                object oldPtr = pt.UserPersistentData;
+
+                                if (oldPtr != null)
+                                {
+                                        if (pt.UserPersistentData != null && _contactDestroyedCallback != null)
+                                        {
+                                                _contactDestroyedCallback(pt.UserPersistentData);
+                                                pt.UserPersistentData = null;
+                                        }
+                                }
+                        }
+                }
+
+                // sort cached points so most isolated points come first
+                private int SortCachedPoints(ManifoldPoint pt)
+                {
+                        //calculate 4 possible cases areas, and take biggest area
+                        //also need to keep 'deepest'
+
+                        int maxPenetrationIndex = -1;
+                        float maxPenetration = pt.Distance;
+                        for (int i = 0; i < 4; i++)
+                        {
+                                if (_pointCache[i].Distance < maxPenetration)
+                                {
+                                        maxPenetrationIndex = i;
+                                        maxPenetration = _pointCache[i].Distance;
+                                }
+                        }
+
+                        float res0 = 0, res1 = 0, res2 = 0, res3 = 0;
+                        if (maxPenetrationIndex != 0)
+                        {
+                                Vector3 a0 = pt.LocalPointA - _pointCache[1].LocalPointA;
+                                Vector3 b0 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA;
+                                Vector3 cross = Vector3.Cross(a0, b0);
+                                res0 = cross.LengthSquared();
+                        }
+                        if (maxPenetrationIndex != 1)
+                        {
+                                Vector3 a1 = pt.LocalPointA - _pointCache[0].LocalPointA;
+                                Vector3 b1 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA;
+                                Vector3 cross = Vector3.Cross(a1, b1);
+                                res1 = cross.LengthSquared();
+                        }
+
+                        if (maxPenetrationIndex != 2)
+                        {
+                                Vector3 a2 = pt.LocalPointA - _pointCache[0].LocalPointA;
+                                Vector3 b2 = _pointCache[3].LocalPointA - _pointCache[1].LocalPointA;
+                                Vector3 cross = Vector3.Cross(a2, b2);
+                                res2 = cross.LengthSquared();
+                        }
+
+                        if (maxPenetrationIndex != 3)
+                        {
+                                Vector3 a3 = pt.LocalPointA - _pointCache[0].LocalPointA;
+                                Vector3 b3 = _pointCache[2].LocalPointA - _pointCache[1].LocalPointA;
+                                Vector3 cross = Vector3.Cross(a3, b3);
+                                res3 = cross.LengthSquared();
+                        }
+
+                        Vector4 maxvec = new Vector4(res0, res1, res2, res3);
+                        int biggestarea = MathHelper.ClosestAxis(maxvec);
+                        return biggestarea;
+                }
+
+                private int FindContactPoint(ManifoldPoint unUsed, int numUnused, ManifoldPoint pt) { return 0; }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs
index 4e0b0a0..c70741e 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs
@@ -1,64 +1,64 @@
-/*

-  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru

-  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

-

-  This software is provided 'as-is', without any express or implied

-  warranty.  In no event will the authors be held liable for any damages

-  arising from the use of this software.

-

-  Permission is granted to anyone to use this software for any purpose,

-  including commercial applications, and to alter it and redistribute it

-  freely, subject to the following restrictions:

-

-  1. The origin of this software must not be misrepresented; you must not

-     claim that you wrote the original software. If you use this software

-     in a product, an acknowledgment in the product documentation would be

-     appreciated but is not required.

-  2. Altered source versions must be plainly marked as such, and must not be

-     misrepresented as being the original software.

-  3. This notice may not be removed or altered from any source distribution.

-*/

-

-using System;

-using System.Collections.Generic;

-using System.Text;

-using MonoXnaCompactMaths;

-

-namespace XnaDevRu.BulletX

-{

-        public class PointCollector : DiscreteCollisionDetectorInterface.Result

-        {

-                private Vector3 _normalOnBInWorld;

-                private Vector3 _pointInWorld;

-                private float _distance; //negative means penetration

-                private bool _hasResult;

-

-                public PointCollector()

-                {

-                        _distance = 1e30f;

-                        _hasResult = false;

-                }

-

-                public Vector3 NormalOnBInWorld { get { return _normalOnBInWorld; } }

-                public Vector3 PointInWorld { get { return _pointInWorld; } }

-                public float Distance { get { return _distance; } }

-                public bool HasResult { get { return _hasResult; } }

-

-                public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB)

-                {

-                        //??

-                }

-

-                public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth)

-                {

-                        if (depth < _distance)

-                        {

-                                _hasResult = true;

-                                _normalOnBInWorld = normalOnBInWorld;

-                                _pointInWorld = pointInWorld;

-                                //negative means penetration

-                                _distance = depth;

-                        }

-                }

-        }

-}

+/*
+  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
+  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using MonoXnaCompactMaths;
+
+namespace XnaDevRu.BulletX
+{
+        public class PointCollector : DiscreteCollisionDetectorInterface.Result
+        {
+                private Vector3 _normalOnBInWorld;
+                private Vector3 _pointInWorld;
+                private float _distance; //negative means penetration
+                private bool _hasResult;
+
+                public PointCollector()
+                {
+                        _distance = 1e30f;
+                        _hasResult = false;
+                }
+
+                public Vector3 NormalOnBInWorld { get { return _normalOnBInWorld; } }
+                public Vector3 PointInWorld { get { return _pointInWorld; } }
+                public float Distance { get { return _distance; } }
+                public bool HasResult { get { return _hasResult; } }
+
+                public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB)
+                {
+                        //??
+                }
+
+                public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth)
+                {
+                        if (depth < _distance)
+                        {
+                                _hasResult = true;
+                                _normalOnBInWorld = normalOnBInWorld;
+                                _pointInWorld = pointInWorld;
+                                //negative means penetration
+                                _distance = depth;
+                        }
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs
index 6d320c4..5c92a0c 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs
@@ -1,142 +1,142 @@
-/*

-  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru

-  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

-

-  This software is provided 'as-is', without any express or implied

-  warranty.  In no event will the authors be held liable for any damages

-  arising from the use of this software.

-

-  Permission is granted to anyone to use this software for any purpose,

-  including commercial applications, and to alter it and redistribute it

-  freely, subject to the following restrictions:

-

-  1. The origin of this software must not be misrepresented; you must not

-     claim that you wrote the original software. If you use this software

-     in a product, an acknowledgment in the product documentation would be

-     appreciated but is not required.

-  2. Altered source versions must be plainly marked as such, and must not be

-     misrepresented as being the original software.

-  3. This notice may not be removed or altered from any source distribution.

-*/

-

-using System;

-using System.Collections.Generic;

-using System.Text;

-using MonoXnaCompactMaths;

-

-namespace XnaDevRu.BulletX

-{

-        /// <summary>

-        /// SubsimplexConvexCast implements Gino van den Bergens' paper

-        /// "Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection"

-        /// GJK based Ray Cast, optimized version

-        /// Objects should not start in overlap, otherwise results are not defined.

-        /// </summary>

-        public class SubsimplexConvexCast : IConvexCast

-        {

-                private ISimplexSolver _simplexSolver;

-                private ConvexShape _convexA;

-                private ConvexShape _convexB;

-

-                /// <summary>

-                /// Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.

-                /// See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565

-                /// </summary>

-                private const int MaxIterations = 32;

-

-                public SubsimplexConvexCast(ConvexShape shapeA, ConvexShape shapeB, ISimplexSolver simplexSolver)

-                {

-                        _simplexSolver = simplexSolver;

-                        _convexA = shapeA;

-                        _convexB = shapeB;

-                }

-

-                #region IConvexCast Members

-                /// <summary>

-                /// SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.

-                /// Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using GjkPairDetector.

-                /// </summary>

-                /// <param name="fromA"></param>

-                /// <param name="toA"></param>

-                /// <param name="fromB"></param>

-                /// <param name="toB"></param>

-                /// <param name="result"></param>

-                /// <returns></returns>

-                public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)

-                {

-                        MinkowskiSumShape convex = new MinkowskiSumShape(_convexA, _convexB);

-

-                        Matrix rayFromLocalA;

-                        Matrix rayToLocalA;

-

-            rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;

-            rayToLocalA = MathHelper.InvertMatrix(toA) * toB;

-

-                        _simplexSolver.Reset();

-

-                        convex.TransformB = rayFromLocalA;

-

-                        float lambda = 0;

-                        //todo: need to verify this:

-                        //because of minkowski difference, we need the inverse direction

-

-                        Vector3 s = -rayFromLocalA.Translation;

-                        Vector3 r = -(rayToLocalA.Translation - rayFromLocalA.Translation);

-                        Vector3 x = s;

-                        Vector3 v;

-                        Vector3 arbitraryPoint = convex.LocalGetSupportingVertex(r);

-

-                        v = x - arbitraryPoint;

-

-                        int maxIter = MaxIterations;

-

-                        Vector3 n = new Vector3();

-                        float lastLambda = lambda;

-

-                        float dist2 = v.LengthSquared();

-                        float epsilon = 0.0001f;

-

-                        Vector3 w, p;

-                        float VdotR;

-

-                        while ((dist2 > epsilon) && (maxIter-- != 0))

-                        {

-                                p = convex.LocalGetSupportingVertex(v);

-                                w = x - p;

-

-                                float VdotW = Vector3.Dot(v, w);

-

-                                if (VdotW > 0)

-                                {

-                                        VdotR = Vector3.Dot(v, r);

-

-                                        if (VdotR >= -(MathHelper.Epsilon * MathHelper.Epsilon))

-                                                return false;

-                                        else

-                                        {

-                                                lambda = lambda - VdotW / VdotR;

-                                                x = s + lambda * r;

-                                                _simplexSolver.Reset();

-                                                //check next line

-                                                w = x - p;

-                                                lastLambda = lambda;

-                                                n = v;

-                                        }

-                                }

-                                _simplexSolver.AddVertex(w, x, p);

-                                if (_simplexSolver.Closest(out v))

-                                {

-                                        dist2 = v.LengthSquared();

-                                }

-                                else

-                                {

-                                        dist2 = 0f;

-                                }

-                        }

-                        result.Fraction = lambda;

-                        result.Normal = n;

-                        return true;

-                }

-                #endregion

-        }

-}

+/*
+  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
+  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using MonoXnaCompactMaths;
+
+namespace XnaDevRu.BulletX
+{
+        /// <summary>
+        /// SubsimplexConvexCast implements Gino van den Bergens' paper
+        /// "Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection"
+        /// GJK based Ray Cast, optimized version
+        /// Objects should not start in overlap, otherwise results are not defined.
+        /// </summary>
+        public class SubsimplexConvexCast : IConvexCast
+        {
+                private ISimplexSolver _simplexSolver;
+                private ConvexShape _convexA;
+                private ConvexShape _convexB;
+
+                /// <summary>
+                /// Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.
+                /// See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
+                /// </summary>
+                private const int MaxIterations = 32;
+
+                public SubsimplexConvexCast(ConvexShape shapeA, ConvexShape shapeB, ISimplexSolver simplexSolver)
+                {
+                        _simplexSolver = simplexSolver;
+                        _convexA = shapeA;
+                        _convexB = shapeB;
+                }
+
+                #region IConvexCast Members
+                /// <summary>
+                /// SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
+                /// Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using GjkPairDetector.
+                /// </summary>
+                /// <param name="fromA"></param>
+                /// <param name="toA"></param>
+                /// <param name="fromB"></param>
+                /// <param name="toB"></param>
+                /// <param name="result"></param>
+                /// <returns></returns>
+                public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
+                {
+                        MinkowskiSumShape convex = new MinkowskiSumShape(_convexA, _convexB);
+
+                        Matrix rayFromLocalA;
+                        Matrix rayToLocalA;
+
+            rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;
+            rayToLocalA = MathHelper.InvertMatrix(toA) * toB;
+
+                        _simplexSolver.Reset();
+
+                        convex.TransformB = rayFromLocalA;
+
+                        float lambda = 0;
+                        //todo: need to verify this:
+                        //because of minkowski difference, we need the inverse direction
+
+                        Vector3 s = -rayFromLocalA.Translation;
+                        Vector3 r = -(rayToLocalA.Translation - rayFromLocalA.Translation);
+                        Vector3 x = s;
+                        Vector3 v;
+                        Vector3 arbitraryPoint = convex.LocalGetSupportingVertex(r);
+
+                        v = x - arbitraryPoint;
+
+                        int maxIter = MaxIterations;
+
+                        Vector3 n = new Vector3();
+                        float lastLambda = lambda;
+
+                        float dist2 = v.LengthSquared();
+                        float epsilon = 0.0001f;
+
+                        Vector3 w, p;
+                        float VdotR;
+
+                        while ((dist2 > epsilon) && (maxIter-- != 0))
+                        {
+                                p = convex.LocalGetSupportingVertex(v);
+                                w = x - p;
+
+                                float VdotW = Vector3.Dot(v, w);
+
+                                if (VdotW > 0)
+                                {
+                                        VdotR = Vector3.Dot(v, r);
+
+                                        if (VdotR >= -(MathHelper.Epsilon * MathHelper.Epsilon))
+                                                return false;
+                                        else
+                                        {
+                                                lambda = lambda - VdotW / VdotR;
+                                                x = s + lambda * r;
+                                                _simplexSolver.Reset();
+                                                //check next line
+                                                w = x - p;
+                                                lastLambda = lambda;
+                                                n = v;
+                                        }
+                                }
+                                _simplexSolver.AddVertex(w, x, p);
+                                if (_simplexSolver.Closest(out v))
+                                {
+                                        dist2 = v.LengthSquared();
+                                }
+                                else
+                                {
+                                        dist2 = 0f;
+                                }
+                        }
+                        result.Fraction = lambda;
+                        result.Normal = n;
+                        return true;
+                }
+                #endregion
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs
index c127460..1e0fb3b 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs
@@ -1,115 +1,115 @@
-/*

-  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru

-  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

-

-  This software is provided 'as-is', without any express or implied

-  warranty.  In no event will the authors be held liable for any damages

-  arising from the use of this software.

-

-  Permission is granted to anyone to use this software for any purpose,

-  including commercial applications, and to alter it and redistribute it

-  freely, subject to the following restrictions:

-

-  1. The origin of this software must not be misrepresented; you must not

-     claim that you wrote the original software. If you use this software

-     in a product, an acknowledgment in the product documentation would be

-     appreciated but is not required.

-  2. Altered source versions must be plainly marked as such, and must not be

-     misrepresented as being the original software.

-  3. This notice may not be removed or altered from any source distribution.

-*/

-

-using System;

-using System.Collections.Generic;

-using System.Text;

-using MonoXnaCompactMaths;

-

-namespace XnaDevRu.BulletX

-{

-        public abstract class TriangleRaycastCallback : ITriangleCallback

-        {

-                private Vector3 _from;

-                private Vector3 _to;

-                private float _hitFraction;

-

-                public TriangleRaycastCallback(Vector3 from, Vector3 to)

-                {

-                        _from = from;

-                        _to = to;

-                        _hitFraction = 1;

-                }

-

-                public Vector3 From { get { return _from; } set { _from = value; } }

-                public Vector3 To { get { return _to; } set { _to = value; } }

-                public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } }

-

-                public abstract float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex);

-

-                #region ITriangleCallback Members

-

-                public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex)

-                {

-                        Vector3 vertA = triangle[0];

-                        Vector3 vertB = triangle[1];

-                        Vector3 vertC = triangle[2];

-

-                        Vector3 vBA = vertB - vertA;

-                        Vector3 vCA = vertC - vertA;

-

-                        Vector3 triangleNormal = Vector3.Cross(vBA, vCA);

-

-                        float dist = Vector3.Dot(vertA, triangleNormal);

-                        float distA = Vector3.Dot(triangleNormal, _from);

-                        distA -= dist;

-                        float distB = Vector3.Dot(triangleNormal, _to);

-                        distB -= dist;

-

-                        if (distA * distB >= 0.0f)

-                        {

-                                return; // same sign

-                        }

-

-                        float projLength = distA - distB;

-                        float distance = (distA) / (projLength);

-                        // Now we have the intersection point on the plane, we'll see if it's inside the triangle

-                        // Add an epsilon as a tolerance for the raycast,

-                        // in case the ray hits exacly on the edge of the triangle.

-                        // It must be scaled for the triangle size.

-

-                        if (distance < _hitFraction)

-                        {

-                                float edgeTolerance = triangleNormal.LengthSquared();

-                                edgeTolerance *= -0.0001f;

-                                Vector3 point = new Vector3();

-                                MathHelper.SetInterpolate3(_from, _to, distance, ref point);

-

-                                Vector3 vertexAPoint = vertA - point;

-                                Vector3 vertexBPoint = vertB - point;

-                                Vector3 contactPointA = Vector3.Cross(vertexAPoint, vertexBPoint);

-

-                                if (Vector3.Dot(contactPointA, triangleNormal) >= edgeTolerance)

-                                {

-                                        Vector3 vertexCPoint = vertC - point;

-                                        Vector3 contactPointB = Vector3.Cross(vertexBPoint, vertexCPoint);

-                                        if (Vector3.Dot(contactPointB, triangleNormal) >= edgeTolerance)

-                                        {

-                                                Vector3 contactPointC = Vector3.Cross(vertexCPoint, vertexAPoint);

-

-                                                if (Vector3.Dot(contactPointC, triangleNormal) >= edgeTolerance)

-                                                {

-                                                        if (distA > 0)

-                                                        {

-                                                                _hitFraction = ReportHit(triangleNormal, distance, partID, triangleIndex);

-                                                        }

-                                                        else

-                                                        {

-                                                                _hitFraction = ReportHit(-triangleNormal, distance, partID, triangleIndex);

-                                                        }

-                                                }

-                                        }

-                                }

-                        }

-                }

-                #endregion

-        }

-}

+/*
+  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
+  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using MonoXnaCompactMaths;
+
+namespace XnaDevRu.BulletX
+{
+        public abstract class TriangleRaycastCallback : ITriangleCallback
+        {
+                private Vector3 _from;
+                private Vector3 _to;
+                private float _hitFraction;
+
+                public TriangleRaycastCallback(Vector3 from, Vector3 to)
+                {
+                        _from = from;
+                        _to = to;
+                        _hitFraction = 1;
+                }
+
+                public Vector3 From { get { return _from; } set { _from = value; } }
+                public Vector3 To { get { return _to; } set { _to = value; } }
+                public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } }
+
+                public abstract float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex);
+
+                #region ITriangleCallback Members
+
+                public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex)
+                {
+                        Vector3 vertA = triangle[0];
+                        Vector3 vertB = triangle[1];
+                        Vector3 vertC = triangle[2];
+
+                        Vector3 vBA = vertB - vertA;
+                        Vector3 vCA = vertC - vertA;
+
+                        Vector3 triangleNormal = Vector3.Cross(vBA, vCA);
+
+                        float dist = Vector3.Dot(vertA, triangleNormal);
+                        float distA = Vector3.Dot(triangleNormal, _from);
+                        distA -= dist;
+                        float distB = Vector3.Dot(triangleNormal, _to);
+                        distB -= dist;
+
+                        if (distA * distB >= 0.0f)
+                        {
+                                return; // same sign
+                        }
+
+                        float projLength = distA - distB;
+                        float distance = (distA) / (projLength);
+                        // Now we have the intersection point on the plane, we'll see if it's inside the triangle
+                        // Add an epsilon as a tolerance for the raycast,
+                        // in case the ray hits exacly on the edge of the triangle.
+                        // It must be scaled for the triangle size.
+
+                        if (distance < _hitFraction)
+                        {
+                                float edgeTolerance = triangleNormal.LengthSquared();
+                                edgeTolerance *= -0.0001f;
+                                Vector3 point = new Vector3();
+                                MathHelper.SetInterpolate3(_from, _to, distance, ref point);
+
+                                Vector3 vertexAPoint = vertA - point;
+                                Vector3 vertexBPoint = vertB - point;
+                                Vector3 contactPointA = Vector3.Cross(vertexAPoint, vertexBPoint);
+
+                                if (Vector3.Dot(contactPointA, triangleNormal) >= edgeTolerance)
+                                {
+                                        Vector3 vertexCPoint = vertC - point;
+                                        Vector3 contactPointB = Vector3.Cross(vertexBPoint, vertexCPoint);
+                                        if (Vector3.Dot(contactPointB, triangleNormal) >= edgeTolerance)
+                                        {
+                                                Vector3 contactPointC = Vector3.Cross(vertexCPoint, vertexAPoint);
+
+                                                if (Vector3.Dot(contactPointC, triangleNormal) >= edgeTolerance)
+                                                {
+                                                        if (distA > 0)
+                                                        {
+                                                                _hitFraction = ReportHit(triangleNormal, distance, partID, triangleIndex);
+                                                        }
+                                                        else
+                                                        {
+                                                                _hitFraction = ReportHit(-triangleNormal, distance, partID, triangleIndex);
+                                                        }
+                                                }
+                                        }
+                                }
+                        }
+                }
+                #endregion
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs
index 16f3dab..1bc94d3 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs
@@ -1,643 +1,643 @@
-/*

-  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru

-  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com

-

-  This software is provided 'as-is', without any express or implied

-  warranty.  In no event will the authors be held liable for any damages

-  arising from the use of this software.

-

-  Permission is granted to anyone to use this software for any purpose,

-  including commercial applications, and to alter it and redistribute it

-  freely, subject to the following restrictions:

-

-  1. The origin of this software must not be misrepresented; you must not

-     claim that you wrote the original software. If you use this software

-     in a product, an acknowledgment in the product documentation would be

-     appreciated but is not required.

-  2. Altered source versions must be plainly marked as such, and must not be

-     misrepresented as being the original software.

-  3. This notice may not be removed or altered from any source distribution.

-*/

-

-using System;

-using System.Collections.Generic;

-using System.Text;

-using MonoXnaCompactMaths;

-

-namespace XnaDevRu.BulletX

-{

-        public class UsageBitfield

-        {

-                private bool _usedVertexA, _usedVertexB, _usedVertexC, _usedVertexD;

-

-                public bool UsedVertexA { get { return _usedVertexA; } set { _usedVertexA = value; } }

-                public bool UsedVertexB { get { return _usedVertexB; } set { _usedVertexB = value; } }

-                public bool UsedVertexC { get { return _usedVertexC; } set { _usedVertexC = value; } }

-                public bool UsedVertexD { get { return _usedVertexD; } set { _usedVertexD = value; } }

-

-                public void Reset()

-                {

-                        _usedVertexA = _usedVertexB = _usedVertexC = _usedVertexD = false;

-                }

-        }

-

-        public class SubSimplexClosestResult

-        {

-                private Vector3 _closestPointOnSimplex;

-

-                //MASK for m_usedVertices

-                //stores the simplex vertex-usage, using the MASK, 

-                // if m_usedVertices & MASK then the related vertex is used

-                private UsageBitfield _usedVertices = new UsageBitfield();

-                private float[] _barycentricCoords = new float[4];

-                private bool _degenerate;

-

-                public Vector3 ClosestPointOnSimplex { get { return _closestPointOnSimplex; } set { _closestPointOnSimplex = value; } }

-                public UsageBitfield UsedVertices { get { return _usedVertices; } set { _usedVertices = value; } }

-                public float[] BarycentricCoords { get { return _barycentricCoords; } set { _barycentricCoords = value; } }

-                public bool Degenerate { get { return _degenerate; } set { _degenerate = value; } }

-

-                public void Reset()

-                {

-                        _degenerate = false;

-                        SetBarycentricCoordinates();

-                        _usedVertices.Reset();

-                }

-

-                public bool IsValid

-                {

-                        get

-                        {

-                                return (_barycentricCoords[0] >= 0f) &&

-                                                (_barycentricCoords[1] >= 0f) &&

-                                                (_barycentricCoords[2] >= 0f) &&

-                                                (_barycentricCoords[3] >= 0f);

-                        }

-                }

-

-                public void SetBarycentricCoordinates()

-                {

-                        SetBarycentricCoordinates(0f, 0f, 0f, 0f);

-                }

-

-                public void SetBarycentricCoordinates(float a, float b, float c, float d)

-                {

-                        _barycentricCoords[0] = a;

-                        _barycentricCoords[1] = b;

-                        _barycentricCoords[2] = c;

-                        _barycentricCoords[3] = d;

-                }

-        }

-

-        /// VoronoiSimplexSolver is an implementation of the closest point distance

-        /// algorithm from a 1-4 points simplex to the origin.

-        /// Can be used with GJK, as an alternative to Johnson distance algorithm. 

-        public class VoronoiSimplexSolver : ISimplexSolver

-        {

-                private const int VertexA = 0, VertexB = 1, VertexC = 2, VertexD = 3;

-

-                private const int VoronoiSimplexMaxVerts = 5;

-                private const bool CatchDegenerateTetrahedron = true;

-

-                private int _numVertices;

-

-                private Vector3[] _simplexVectorW = new Vector3[VoronoiSimplexMaxVerts];

-                private Vector3[] _simplexPointsP = new Vector3[VoronoiSimplexMaxVerts];

-                private Vector3[] _simplexPointsQ = new Vector3[VoronoiSimplexMaxVerts];

-

-                private Vector3 _cachedPA;

-                private Vector3 _cachedPB;

-                private Vector3 _cachedV;

-                private Vector3 _lastW;

-                private bool _cachedValidClosest;

-

-                private SubSimplexClosestResult _cachedBC = new SubSimplexClosestResult();

-

-                private bool _needsUpdate;

-        

-                #region ISimplexSolver Members

-

-        public bool FullSimplex

-                {

-                        get

-                        {

-                                return _numVertices == 4;

-                        }

-                }

-

-        public int NumVertices

-                {

-                        get

-                        {

-                                return _numVertices;

-                        }

-                }

-

-                public void  Reset()

-                {

-                        _cachedValidClosest = false;

-                        _numVertices = 0;

-                        _needsUpdate = true;

-                        _lastW = new Vector3(1e30f, 1e30f, 1e30f);

-                        _cachedBC.Reset();

-                }

-

-        public void AddVertex(Vector3 w, Vector3 p, Vector3 q)

-                {

-                        _lastW = w;

-                        _needsUpdate = true;

-

-                        _simplexVectorW[_numVertices] = w;

-                        _simplexPointsP[_numVertices] = p;

-                        _simplexPointsQ[_numVertices] = q;

-

-                        _numVertices++;

-                }

-

-                //return/calculate the closest vertex

-        public bool Closest(out Vector3 v)

-                {

-                        bool succes = UpdateClosestVectorAndPoints();

-                        v = _cachedV;

-                        return succes;

-                }

-

-        public float MaxVertex

-                {

-                        get

-                        {

-                                int numverts = NumVertices;

-                                float maxV = 0f, curLen2;

-                                for (int i = 0; i < numverts; i++)

-                                {

-                                        curLen2 = _simplexVectorW[i].LengthSquared();

-                                        if (maxV < curLen2) maxV = curLen2;

-                                }

-                                return maxV;

-                        }

-                }

-

-                //return the current simplex

-        public int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf)

-                {

-                        int numverts = NumVertices;

-                        pBuf = new Vector3[numverts];

-                        qBuf = new Vector3[numverts];

-                        yBuf = new Vector3[numverts];

-                        for (int i = 0; i < numverts; i++)

-                        {

-                                yBuf[i] = _simplexVectorW[i];

-                                pBuf[i] = _simplexPointsP[i];

-                                qBuf[i] = _simplexPointsQ[i];

-                        }

-                        return numverts;

-                }

-

-                public  bool  InSimplex(Vector3 w)

-                {

-                        //check in case lastW is already removed

-                        if (w == _lastW) return true;

-

-                        //w is in the current (reduced) simplex

-                        int numverts = NumVertices;

-                        for (int i = 0; i < numverts; i++)

-                                if (_simplexVectorW[i] == w) return true;

-                                                

-                        return false;

-                }

-

-        public void BackupClosest(out Vector3 v)

-                {

-                        v = _cachedV;

-                }

-

-        public bool EmptySimplex

-                {

-                        get

-                        {

-                                return NumVertices == 0;

-                        }

-                }

-

-        public void ComputePoints(out Vector3 p1, out Vector3 p2)

-                {

-                        UpdateClosestVectorAndPoints();

-                        p1 = _cachedPA;

-                        p2 = _cachedPB;

-                }

-

-                #endregion

-                

-                public void RemoveVertex(int index)

-                {

-            BulletDebug.Assert(_numVertices > 0);

-                        _numVertices--;

-                        _simplexVectorW[index] = _simplexVectorW[_numVertices];

-                        _simplexPointsP[index] = _simplexPointsP[_numVertices];

-                        _simplexPointsQ[index] = _simplexPointsQ[_numVertices];

-                }

-

-                public void ReduceVertices(UsageBitfield usedVerts)

-                {

-                        if ((NumVertices >= 4) && (!usedVerts.UsedVertexD)) RemoveVertex(3);

-                        if ((NumVertices >= 3) && (!usedVerts.UsedVertexC)) RemoveVertex(2);

-                        if ((NumVertices >= 2) && (!usedVerts.UsedVertexB)) RemoveVertex(1);

-                        if ((NumVertices >= 1) && (!usedVerts.UsedVertexA)) RemoveVertex(0);

-                }

-

-                public bool UpdateClosestVectorAndPoints()

-                {       

-                        if (_needsUpdate)

-                        {

-                                _cachedBC.Reset();

-                                _needsUpdate = false;

-

-                                Vector3 p, a, b, c, d;

-                                switch (NumVertices)

-                                {

-                                        case 0:

-                                                _cachedValidClosest = false;

-                                                break;

-                                        case 1:

-                                                _cachedPA = _simplexPointsP[0];

-                                                _cachedPB = _simplexPointsQ[0];

-                                                _cachedV = _cachedPA - _cachedPB;

-                                                _cachedBC.Reset();

-                                                _cachedBC.SetBarycentricCoordinates(1f, 0f, 0f, 0f);

-                                                _cachedValidClosest = _cachedBC.IsValid;

-                                                break;

-                                        case 2:

-                                                //closest point origin from line segment

-                                                Vector3 from = _simplexVectorW[0];

-                                                Vector3 to = _simplexVectorW[1];

-                                                Vector3 nearest;

-

-                                                Vector3 diff = -from;

-                                                Vector3 v = to - from;

-                                                float t = Vector3.Dot(v, diff);

-                                                        

-                                                if (t > 0)

-                                                {

-                                                        float dotVV = v.LengthSquared();

-                                                        if (t < dotVV)

-                                                        {

-                                                                t /= dotVV;

-                                                                diff -= t * v;

-                                                                _cachedBC.UsedVertices.UsedVertexA = true;

-                                                                _cachedBC.UsedVertices.UsedVertexB = true;

-                                                        }

-                                                        else

-                                                        {

-                                                                t = 1;

-                                                                diff -= v;

-                                                                //reduce to 1 point

-                                                                _cachedBC.UsedVertices.UsedVertexB = true;

-                                                        }

-                                                }

-                                                else

-                                                {

-                                                        t = 0;

-                                                        //reduce to 1 point

-                                                        _cachedBC.UsedVertices.UsedVertexA = true;

-                                                }

-

-                                                _cachedBC.SetBarycentricCoordinates(1 - t, t, 0, 0);

-                                                nearest = from + t * v;

-

-                                                _cachedPA = _simplexPointsP[0] + t * (_simplexPointsP[1] - _simplexPointsP[0]);

-                                                _cachedPB = _simplexPointsQ[0] + t * (_simplexPointsQ[1] - _simplexPointsQ[0]);

-                                                _cachedV = _cachedPA - _cachedPB;

-

-                                                ReduceVertices(_cachedBC.UsedVertices);

-

-                                                _cachedValidClosest = _cachedBC.IsValid;

-                                                break;

-                                        case 3:

-                                                //closest point origin from triangle

-                                                p = new Vector3();

-                                                a = _simplexVectorW[0];

-                                                b = _simplexVectorW[1];

-                                                c = _simplexVectorW[2];

-

-                                                ClosestPtPointTriangle(p, a, b, c, ref _cachedBC);

-                                                _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] +

-                                                                                _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] +

-                                                                                _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] +

-                                                                                _simplexPointsP[3] * _cachedBC.BarycentricCoords[3];

-

-                                                _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] +

-                                                                                _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] +

-                                                                                _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] +

-                                                                                _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3];

-

-                                                _cachedV = _cachedPA - _cachedPB;

-

-                                                ReduceVertices(_cachedBC.UsedVertices);

-                                                _cachedValidClosest = _cachedBC.IsValid;

-                                                break;

-                                        case 4:

-                                                p = new Vector3();

-                                                a = _simplexVectorW[0];

-                                                b = _simplexVectorW[1];

-                                                c = _simplexVectorW[2];

-                                                d = _simplexVectorW[3];

-

-                                                bool hasSeperation = ClosestPtPointTetrahedron(p, a, b, c, d, ref _cachedBC);

-

-                                                if (hasSeperation)

-                                                {

-                                                        _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] +

-                                                                                        _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] +

-                                                                                        _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] +

-                                                                                        _simplexPointsP[3] * _cachedBC.BarycentricCoords[3];

-

-                                                        _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] +

-                                                                                        _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] +

-                                                                                        _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] +

-                                                                                        _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3];

-

-                                                        _cachedV = _cachedPA - _cachedPB;

-                                                        ReduceVertices(_cachedBC.UsedVertices);

-                                                }

-                                                else

-                                                {

-                                                        if (_cachedBC.Degenerate)

-                                                        {

-                                                                _cachedValidClosest = false;

-                                                        }

-                                                        else

-                                                        {

-                                                                _cachedValidClosest = true;

-                                                                //degenerate case == false, penetration = true + zero

-                                                                _cachedV.X = _cachedV.Y = _cachedV.Z = 0f;

-                                                        }

-                                                        break; // !!!!!!!!!!!! proverit na vsakiy sluchai

-                                                }

-

-                                                _cachedValidClosest = _cachedBC.IsValid;

-

-                                                //closest point origin from tetrahedron

-                                                break;

-                                        default:

-                                                _cachedValidClosest = false;

-                                                break;

-                                }

-                        }

-

-                        return _cachedValidClosest;

-                }

-

-                public bool ClosestPtPointTriangle(Vector3 p, Vector3 a, Vector3 b, Vector3 c,

-                        ref SubSimplexClosestResult result)

-                {

-                        result.UsedVertices.Reset();

-

-                        float v, w;

-

-                        // Check if P in vertex region outside A

-                        Vector3 ab = b - a;

-                        Vector3 ac = c - a;

-                        Vector3 ap = p - a;

-                        float d1 = Vector3.Dot(ab, ap);

-                        float d2 = Vector3.Dot(ac, ap);

-                        if (d1 <= 0f && d2 <= 0f) 

-                        {

-                                result.ClosestPointOnSimplex = a;

-                                result.UsedVertices.UsedVertexA = true;

-                                result.SetBarycentricCoordinates(1, 0, 0, 0);

-                                return true; // a; // barycentric coordinates (1,0,0)

-                        }

-

-                        // Check if P in vertex region outside B

-                        Vector3 bp = p - b;

-                        float d3 = Vector3.Dot(ab, bp);

-                        float d4 = Vector3.Dot(ac, bp);

-                        if (d3 >= 0f && d4 <= d3) 

-                        {

-                                result.ClosestPointOnSimplex = b;

-                                result.UsedVertices.UsedVertexB = true;

-                                result.SetBarycentricCoordinates(0, 1, 0, 0);

-

-                                return true; // b; // barycentric coordinates (0,1,0)

-                        }

-                        // Check if P in edge region of AB, if so return projection of P onto AB

-                        float vc = d1 * d4 - d3 * d2;

-                        if (vc <= 0f && d1 >= 0f && d3 <= 0f)

-                        {

-                                v = d1 / (d1 - d3);

-                                result.ClosestPointOnSimplex = a + v * ab;

-                                result.UsedVertices.UsedVertexA = true;

-                                result.UsedVertices.UsedVertexB = true;

-                                result.SetBarycentricCoordinates(1 - v, v, 0, 0);

-                                return true;

-                                //return a + v * ab; // barycentric coordinates (1-v,v,0)

-                        }

-

-                        // Check if P in vertex region outside C

-                        Vector3 cp = p - c;

-                        float d5 = Vector3.Dot(ab, cp);

-                        float d6 = Vector3.Dot(ac, cp);

-                        if (d6 >= 0f && d5 <= d6) 

-                        {

-                                result.ClosestPointOnSimplex = c;

-                                result.UsedVertices.UsedVertexC = true;

-                                result.SetBarycentricCoordinates(0, 0, 1, 0);

-                                return true;//c; // barycentric coordinates (0,0,1)

-                        }

-

-                        // Check if P in edge region of AC, if so return projection of P onto AC

-                        float vb = d5 * d2 - d1 * d6;

-                        if (vb <= 0f && d2 >= 0f && d6 <= 0f)

-                        {

-                                w = d2 / (d2 - d6);

-                                result.ClosestPointOnSimplex = a + w * ac;

-                                result.UsedVertices.UsedVertexA = true;

-                                result.UsedVertices.UsedVertexC = true;

-                                result.SetBarycentricCoordinates(1 - w, 0, w, 0);

-                                return true;

-                                //return a + w * ac; // barycentric coordinates (1-w,0,w)

-                        }

-

-                        // Check if P in edge region of BC, if so return projection of P onto BC

-                        float va = d3 * d6 - d5 * d4;

-                        if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f)

-                        {

-                                w = (d4 - d3) / ((d4 - d3) + (d5 - d6));

-

-                                result.ClosestPointOnSimplex = b + w * (c - b);

-                                result.UsedVertices.UsedVertexB = true;

-                                result.UsedVertices.UsedVertexC = true;

-                                result.SetBarycentricCoordinates(0, 1 - w, w, 0);

-                                return true;            

-                           // return b + w * (c - b); // barycentric coordinates (0,1-w,w)

-                        }

-

-                        // P inside face region. Compute Q through its barycentric coordinates (u,v,w)

-                        float denom = 1.0f / (va + vb + vc);

-                        v = vb * denom;

-                        w = vc * denom;

-

-                        result.ClosestPointOnSimplex = a + ab * v + ac * w;

-                        result.UsedVertices.UsedVertexA = true;

-                        result.UsedVertices.UsedVertexB = true;

-                        result.UsedVertices.UsedVertexC = true;

-                        result.SetBarycentricCoordinates(1 - v - w, v, w, 0);

-                        

-                        return true;

-                }

-

-                /// Test if point p and d lie on opposite sides of plane through abc

-                public int PointOutsideOfPlane(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d)

-                {

-                        Vector3 normal = Vector3.Cross(b - a, c - a);

-

-                        float signp = Vector3.Dot(p - a, normal); // [AP AB AC]

-                        float signd = Vector3.Dot(d - a, normal); // [AD AB AC]

-

-                        if (CatchDegenerateTetrahedron)

-                                if (signd * signd < (1e-4f * 1e-4f)) return -1;

-

-                        // Points on opposite sides if expression signs are opposite

-                        return signp * signd < 0f ? 1 : 0;

-                }

-                

-                public bool ClosestPtPointTetrahedron(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d,

-                        ref SubSimplexClosestResult finalResult)

-                {

-                        SubSimplexClosestResult tempResult = new SubSimplexClosestResult();

-

-                        // Start out assuming point inside all halfspaces, so closest to itself

-                        finalResult.ClosestPointOnSimplex = p;

-                        finalResult.UsedVertices.Reset();

-                        finalResult.UsedVertices.UsedVertexA = true;

-                        finalResult.UsedVertices.UsedVertexB = true;

-                        finalResult.UsedVertices.UsedVertexC = true;

-                        finalResult.UsedVertices.UsedVertexD = true;

-

-                        int pointOutsideABC = PointOutsideOfPlane(p, a, b, c, d);

-                        int pointOutsideACD = PointOutsideOfPlane(p, a, c, d, b);

-                        int     pointOutsideADB = PointOutsideOfPlane(p, a, d, b, c);

-                        int     pointOutsideBDC = PointOutsideOfPlane(p, b, d, c, a);

-

-                        if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)

-                        {

-                                finalResult.Degenerate = true;

-                                return false;

-                        }

-

-                        if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0)

-                                return false;

-

-                        float bestSqDist = float.MaxValue;

-                        // If point outside face abc then compute closest point on abc

-                        if (pointOutsideABC != 0) 

-                        {

-                                ClosestPtPointTriangle(p, a, b, c, ref tempResult);

-                                Vector3 q = tempResult.ClosestPointOnSimplex;

-

-                                float sqDist = ((Vector3)(q - p)).LengthSquared();

-                                // Update best closest point if (squared) distance is less than current best

-                                if (sqDist < bestSqDist)

-                                {

-                                        bestSqDist = sqDist;

-                                        finalResult.ClosestPointOnSimplex = q;

-                                        //convert result bitmask!

-                                        finalResult.UsedVertices.Reset();

-                                        finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA;

-                                        finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexB;

-                                        finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC;

-                                        finalResult.SetBarycentricCoordinates(

-                                                        tempResult.BarycentricCoords[VertexA],

-                                                        tempResult.BarycentricCoords[VertexB],

-                                                        tempResult.BarycentricCoords[VertexC],

-                                                        0);

-                                }

-                        }

-                  

-                        // Repeat test for face acd

-                        if (pointOutsideACD != 0) 

-                        {

-                                ClosestPtPointTriangle(p, a, c, d, ref tempResult);

-                                Vector3 q = tempResult.ClosestPointOnSimplex;

-                                //convert result bitmask!

-

-                                float sqDist = ((Vector3)(q - p)).LengthSquared();

-                                if (sqDist < bestSqDist) 

-                                {

-                                        bestSqDist = sqDist;

-                                        finalResult.ClosestPointOnSimplex = q;

-                                        finalResult.UsedVertices.Reset();

-                                        finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA;

-                                        finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexB;

-                                        finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexC;

-                                        finalResult.SetBarycentricCoordinates(

-                                                        tempResult.BarycentricCoords[VertexA],

-                                                        0,

-                                                        tempResult.BarycentricCoords[VertexB],

-                                                        tempResult.BarycentricCoords[VertexC]);

-                                }

-                        }

-                        // Repeat test for face adb

-                        

-                        if (pointOutsideADB != 0)

-                        {

-                                ClosestPtPointTriangle(p, a, d, b, ref tempResult);

-                                Vector3 q = tempResult.ClosestPointOnSimplex;

-                                //convert result bitmask!

-

-                                float sqDist = ((Vector3)(q - p)).LengthSquared();

-                                if (sqDist < bestSqDist) 

-                                {

-                                        bestSqDist = sqDist;

-                                        finalResult.ClosestPointOnSimplex = q;

-                                        finalResult.UsedVertices.Reset();

-                                        finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA;

-                                        finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB;

-                                        finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexC;

-                                        finalResult.SetBarycentricCoordinates(

-                                                        tempResult.BarycentricCoords[VertexA],

-                                                        tempResult.BarycentricCoords[VertexC],

-                                                        0,

-                                                        tempResult.BarycentricCoords[VertexB]);

-

-                                }

-                        }

-                        // Repeat test for face bdc

-

-                        if (pointOutsideBDC != 0)

-                        {

-                                ClosestPtPointTriangle(p, b, d, c, ref tempResult);

-                                Vector3 q = tempResult.ClosestPointOnSimplex;

-                                //convert result bitmask!

-                                float sqDist = ((Vector3)(q - p)).LengthSquared();

-                                if (sqDist < bestSqDist) 

-                                {

-                                        bestSqDist = sqDist;

-                                        finalResult.ClosestPointOnSimplex = q;

-                                        finalResult.UsedVertices.Reset();

-                                        finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexA;

-                                        finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB;

-                                        finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC;

-

-                                        finalResult.SetBarycentricCoordinates(

-                                                        0,

-                                                        tempResult.BarycentricCoords[VertexA],

-                                                        tempResult.BarycentricCoords[VertexC],

-                                                        tempResult.BarycentricCoords[VertexB]);

-                                }

-                        }

-

-                        //help! we ended up full !

-

-                        if (finalResult.UsedVertices.UsedVertexA &&

-                                finalResult.UsedVertices.UsedVertexB &&

-                                finalResult.UsedVertices.UsedVertexC &&

-                                finalResult.UsedVertices.UsedVertexD) 

-                        {

-                                return true;

-                        }

-

-                        return true;

-                }

-    }

-}

+/*
+  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
+  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+*/
+
+using System;
+using System.Collections.Generic;
+using System.Text;
+using MonoXnaCompactMaths;
+
+namespace XnaDevRu.BulletX
+{
+        public class UsageBitfield
+        {
+                private bool _usedVertexA, _usedVertexB, _usedVertexC, _usedVertexD;
+
+                public bool UsedVertexA { get { return _usedVertexA; } set { _usedVertexA = value; } }
+                public bool UsedVertexB { get { return _usedVertexB; } set { _usedVertexB = value; } }
+                public bool UsedVertexC { get { return _usedVertexC; } set { _usedVertexC = value; } }
+                public bool UsedVertexD { get { return _usedVertexD; } set { _usedVertexD = value; } }
+
+                public void Reset()
+                {
+                        _usedVertexA = _usedVertexB = _usedVertexC = _usedVertexD = false;
+                }
+        }
+
+        public class SubSimplexClosestResult
+        {
+                private Vector3 _closestPointOnSimplex;
+
+                //MASK for m_usedVertices
+                //stores the simplex vertex-usage, using the MASK, 
+                // if m_usedVertices & MASK then the related vertex is used
+                private UsageBitfield _usedVertices = new UsageBitfield();
+                private float[] _barycentricCoords = new float[4];
+                private bool _degenerate;
+
+                public Vector3 ClosestPointOnSimplex { get { return _closestPointOnSimplex; } set { _closestPointOnSimplex = value; } }
+                public UsageBitfield UsedVertices { get { return _usedVertices; } set { _usedVertices = value; } }
+                public float[] BarycentricCoords { get { return _barycentricCoords; } set { _barycentricCoords = value; } }
+                public bool Degenerate { get { return _degenerate; } set { _degenerate = value; } }
+
+                public void Reset()
+                {
+                        _degenerate = false;
+                        SetBarycentricCoordinates();
+                        _usedVertices.Reset();
+                }
+
+                public bool IsValid
+                {
+                        get
+                        {
+                                return (_barycentricCoords[0] >= 0f) &&
+                                                (_barycentricCoords[1] >= 0f) &&
+                                                (_barycentricCoords[2] >= 0f) &&
+                                                (_barycentricCoords[3] >= 0f);
+                        }
+                }
+
+                public void SetBarycentricCoordinates()
+                {
+                        SetBarycentricCoordinates(0f, 0f, 0f, 0f);
+                }
+
+                public void SetBarycentricCoordinates(float a, float b, float c, float d)
+                {
+                        _barycentricCoords[0] = a;
+                        _barycentricCoords[1] = b;
+                        _barycentricCoords[2] = c;
+                        _barycentricCoords[3] = d;
+                }
+        }
+
+        /// VoronoiSimplexSolver is an implementation of the closest point distance
+        /// algorithm from a 1-4 points simplex to the origin.
+        /// Can be used with GJK, as an alternative to Johnson distance algorithm. 
+        public class VoronoiSimplexSolver : ISimplexSolver
+        {
+                private const int VertexA = 0, VertexB = 1, VertexC = 2, VertexD = 3;
+
+                private const int VoronoiSimplexMaxVerts = 5;
+                private const bool CatchDegenerateTetrahedron = true;
+
+                private int _numVertices;
+
+                private Vector3[] _simplexVectorW = new Vector3[VoronoiSimplexMaxVerts];
+                private Vector3[] _simplexPointsP = new Vector3[VoronoiSimplexMaxVerts];
+                private Vector3[] _simplexPointsQ = new Vector3[VoronoiSimplexMaxVerts];
+
+                private Vector3 _cachedPA;
+                private Vector3 _cachedPB;
+                private Vector3 _cachedV;
+                private Vector3 _lastW;
+                private bool _cachedValidClosest;
+
+                private SubSimplexClosestResult _cachedBC = new SubSimplexClosestResult();
+
+                private bool _needsUpdate;
+        
+                #region ISimplexSolver Members
+
+        public bool FullSimplex
+                {
+                        get
+                        {
+                                return _numVertices == 4;
+                        }
+                }
+
+        public int NumVertices
+                {
+                        get
+                        {
+                                return _numVertices;
+                        }
+                }
+
+                public void  Reset()
+                {
+                        _cachedValidClosest = false;
+                        _numVertices = 0;
+                        _needsUpdate = true;
+                        _lastW = new Vector3(1e30f, 1e30f, 1e30f);
+                        _cachedBC.Reset();
+                }
+
+        public void AddVertex(Vector3 w, Vector3 p, Vector3 q)
+                {
+                        _lastW = w;
+                        _needsUpdate = true;
+
+                        _simplexVectorW[_numVertices] = w;
+                        _simplexPointsP[_numVertices] = p;
+                        _simplexPointsQ[_numVertices] = q;
+
+                        _numVertices++;
+                }
+
+                //return/calculate the closest vertex
+        public bool Closest(out Vector3 v)
+                {
+                        bool succes = UpdateClosestVectorAndPoints();
+                        v = _cachedV;
+                        return succes;
+                }
+
+        public float MaxVertex
+                {
+                        get
+                        {
+                                int numverts = NumVertices;
+                                float maxV = 0f, curLen2;
+                                for (int i = 0; i < numverts; i++)
+                                {
+                                        curLen2 = _simplexVectorW[i].LengthSquared();
+                                        if (maxV < curLen2) maxV = curLen2;
+                                }
+                                return maxV;
+                        }
+                }
+
+                //return the current simplex
+        public int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf)
+                {
+                        int numverts = NumVertices;
+                        pBuf = new Vector3[numverts];
+                        qBuf = new Vector3[numverts];
+                        yBuf = new Vector3[numverts];
+                        for (int i = 0; i < numverts; i++)
+                        {
+                                yBuf[i] = _simplexVectorW[i];
+                                pBuf[i] = _simplexPointsP[i];
+                                qBuf[i] = _simplexPointsQ[i];
+                        }
+                        return numverts;
+                }
+
+                public  bool  InSimplex(Vector3 w)
+                {
+                        //check in case lastW is already removed
+                        if (w == _lastW) return true;
+
+                        //w is in the current (reduced) simplex
+                        int numverts = NumVertices;
+                        for (int i = 0; i < numverts; i++)
+                                if (_simplexVectorW[i] == w) return true;
+                                                
+                        return false;
+                }
+
+        public void BackupClosest(out Vector3 v)
+                {
+                        v = _cachedV;
+                }
+
+        public bool EmptySimplex
+                {
+                        get
+                        {
+                                return NumVertices == 0;
+                        }
+                }
+
+        public void ComputePoints(out Vector3 p1, out Vector3 p2)
+                {
+                        UpdateClosestVectorAndPoints();
+                        p1 = _cachedPA;
+                        p2 = _cachedPB;
+                }
+
+                #endregion
+                
+                public void RemoveVertex(int index)
+                {
+            BulletDebug.Assert(_numVertices > 0);
+                        _numVertices--;
+                        _simplexVectorW[index] = _simplexVectorW[_numVertices];
+                        _simplexPointsP[index] = _simplexPointsP[_numVertices];
+                        _simplexPointsQ[index] = _simplexPointsQ[_numVertices];
+                }
+
+                public void ReduceVertices(UsageBitfield usedVerts)
+                {
+                        if ((NumVertices >= 4) && (!usedVerts.UsedVertexD)) RemoveVertex(3);
+                        if ((NumVertices >= 3) && (!usedVerts.UsedVertexC)) RemoveVertex(2);
+                        if ((NumVertices >= 2) && (!usedVerts.UsedVertexB)) RemoveVertex(1);
+                        if ((NumVertices >= 1) && (!usedVerts.UsedVertexA)) RemoveVertex(0);
+                }
+
+                public bool UpdateClosestVectorAndPoints()
+                {       
+                        if (_needsUpdate)
+                        {
+                                _cachedBC.Reset();
+                                _needsUpdate = false;
+
+                                Vector3 p, a, b, c, d;
+                                switch (NumVertices)
+                                {
+                                        case 0:
+                                                _cachedValidClosest = false;
+                                                break;
+                                        case 1:
+                                                _cachedPA = _simplexPointsP[0];
+                                                _cachedPB = _simplexPointsQ[0];
+                                                _cachedV = _cachedPA - _cachedPB;
+                                                _cachedBC.Reset();
+                                                _cachedBC.SetBarycentricCoordinates(1f, 0f, 0f, 0f);
+                                                _cachedValidClosest = _cachedBC.IsValid;
+                                                break;
+                                        case 2:
+                                                //closest point origin from line segment
+                                                Vector3 from = _simplexVectorW[0];
+                                                Vector3 to = _simplexVectorW[1];
+                                                Vector3 nearest;
+
+                                                Vector3 diff = -from;
+                                                Vector3 v = to - from;
+                                                float t = Vector3.Dot(v, diff);
+                                                        
+                                                if (t > 0)
+                                                {
+                                                        float dotVV = v.LengthSquared();
+                                                        if (t < dotVV)
+                                                        {
+                                                                t /= dotVV;
+                                                                diff -= t * v;
+                                                                _cachedBC.UsedVertices.UsedVertexA = true;
+                                                                _cachedBC.UsedVertices.UsedVertexB = true;
+                                                        }
+                                                        else
+                                                        {
+                                                                t = 1;
+                                                                diff -= v;
+                                                                //reduce to 1 point
+                                                                _cachedBC.UsedVertices.UsedVertexB = true;
+                                                        }
+                                                }
+                                                else
+                                                {
+                                                        t = 0;
+                                                        //reduce to 1 point
+                                                        _cachedBC.UsedVertices.UsedVertexA = true;
+                                                }
+
+                                                _cachedBC.SetBarycentricCoordinates(1 - t, t, 0, 0);
+                                                nearest = from + t * v;
+
+                                                _cachedPA = _simplexPointsP[0] + t * (_simplexPointsP[1] - _simplexPointsP[0]);
+                                                _cachedPB = _simplexPointsQ[0] + t * (_simplexPointsQ[1] - _simplexPointsQ[0]);
+                                                _cachedV = _cachedPA - _cachedPB;
+
+                                                ReduceVertices(_cachedBC.UsedVertices);
+
+                                                _cachedValidClosest = _cachedBC.IsValid;
+                                                break;
+                                        case 3:
+                                                //closest point origin from triangle
+                                                p = new Vector3();
+                                                a = _simplexVectorW[0];
+                                                b = _simplexVectorW[1];
+                                                c = _simplexVectorW[2];
+
+                                                ClosestPtPointTriangle(p, a, b, c, ref _cachedBC);
+                                                _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] +
+                                                                                _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] +
+                                                                                _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] +
+                                                                                _simplexPointsP[3] * _cachedBC.BarycentricCoords[3];
+
+                                                _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] +
+                                                                                _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] +
+                                                                                _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] +
+                                                                                _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3];
+
+                                                _cachedV = _cachedPA - _cachedPB;
+
+                                                ReduceVertices(_cachedBC.UsedVertices);
+                                                _cachedValidClosest = _cachedBC.IsValid;
+                                                break;
+                                        case 4:
+                                                p = new Vector3();
+                                                a = _simplexVectorW[0];
+                                                b = _simplexVectorW[1];
+                                                c = _simplexVectorW[2];
+                                                d = _simplexVectorW[3];
+
+                                                bool hasSeperation = ClosestPtPointTetrahedron(p, a, b, c, d, ref _cachedBC);
+
+                                                if (hasSeperation)
+                                                {
+                                                        _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] +
+                                                                                        _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] +
+                                                                                        _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] +
+                                                                                        _simplexPointsP[3] * _cachedBC.BarycentricCoords[3];
+
+                                                        _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] +
+                                                                                        _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] +
+                                                                                        _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] +
+                                                                                        _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3];
+
+                                                        _cachedV = _cachedPA - _cachedPB;
+                                                        ReduceVertices(_cachedBC.UsedVertices);
+                                                }
+                                                else
+                                                {
+                                                        if (_cachedBC.Degenerate)
+                                                        {
+                                                                _cachedValidClosest = false;
+                                                        }
+                                                        else
+                                                        {
+                                                                _cachedValidClosest = true;
+                                                                //degenerate case == false, penetration = true + zero
+                                                                _cachedV.X = _cachedV.Y = _cachedV.Z = 0f;
+                                                        }
+                                                        break; // !!!!!!!!!!!! proverit na vsakiy sluchai
+                                                }
+
+                                                _cachedValidClosest = _cachedBC.IsValid;
+
+                                                //closest point origin from tetrahedron
+                                                break;
+                                        default:
+                                                _cachedValidClosest = false;
+                                                break;
+                                }
+                        }
+
+                        return _cachedValidClosest;
+                }
+
+                public bool ClosestPtPointTriangle(Vector3 p, Vector3 a, Vector3 b, Vector3 c,
+                        ref SubSimplexClosestResult result)
+                {
+                        result.UsedVertices.Reset();
+
+                        float v, w;
+
+                        // Check if P in vertex region outside A
+                        Vector3 ab = b - a;
+                        Vector3 ac = c - a;
+                        Vector3 ap = p - a;
+                        float d1 = Vector3.Dot(ab, ap);
+                        float d2 = Vector3.Dot(ac, ap);
+                        if (d1 <= 0f && d2 <= 0f) 
+                        {
+                                result.ClosestPointOnSimplex = a;
+                                result.UsedVertices.UsedVertexA = true;
+                                result.SetBarycentricCoordinates(1, 0, 0, 0);
+                                return true; // a; // barycentric coordinates (1,0,0)
+                        }
+
+                        // Check if P in vertex region outside B
+                        Vector3 bp = p - b;
+                        float d3 = Vector3.Dot(ab, bp);
+                        float d4 = Vector3.Dot(ac, bp);
+                        if (d3 >= 0f && d4 <= d3) 
+                        {
+                                result.ClosestPointOnSimplex = b;
+                                result.UsedVertices.UsedVertexB = true;
+                                result.SetBarycentricCoordinates(0, 1, 0, 0);
+
+                                return true; // b; // barycentric coordinates (0,1,0)
+                        }
+                        // Check if P in edge region of AB, if so return projection of P onto AB
+                        float vc = d1 * d4 - d3 * d2;
+                        if (vc <= 0f && d1 >= 0f && d3 <= 0f)
+                        {
+                                v = d1 / (d1 - d3);
+                                result.ClosestPointOnSimplex = a + v * ab;
+                                result.UsedVertices.UsedVertexA = true;
+                                result.UsedVertices.UsedVertexB = true;
+                                result.SetBarycentricCoordinates(1 - v, v, 0, 0);
+                                return true;
+                                //return a + v * ab; // barycentric coordinates (1-v,v,0)
+                        }
+
+                        // Check if P in vertex region outside C
+                        Vector3 cp = p - c;
+                        float d5 = Vector3.Dot(ab, cp);
+                        float d6 = Vector3.Dot(ac, cp);
+                        if (d6 >= 0f && d5 <= d6) 
+                        {
+                                result.ClosestPointOnSimplex = c;
+                                result.UsedVertices.UsedVertexC = true;
+                                result.SetBarycentricCoordinates(0, 0, 1, 0);
+                                return true;//c; // barycentric coordinates (0,0,1)
+                        }
+
+                        // Check if P in edge region of AC, if so return projection of P onto AC
+                        float vb = d5 * d2 - d1 * d6;
+                        if (vb <= 0f && d2 >= 0f && d6 <= 0f)
+                        {
+                                w = d2 / (d2 - d6);
+                                result.ClosestPointOnSimplex = a + w * ac;
+                                result.UsedVertices.UsedVertexA = true;
+                                result.UsedVertices.UsedVertexC = true;
+                                result.SetBarycentricCoordinates(1 - w, 0, w, 0);
+                                return true;
+                                //return a + w * ac; // barycentric coordinates (1-w,0,w)
+                        }
+
+                        // Check if P in edge region of BC, if so return projection of P onto BC
+                        float va = d3 * d6 - d5 * d4;
+                        if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f)
+                        {
+                                w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
+
+                                result.ClosestPointOnSimplex = b + w * (c - b);
+                                result.UsedVertices.UsedVertexB = true;
+                                result.UsedVertices.UsedVertexC = true;
+                                result.SetBarycentricCoordinates(0, 1 - w, w, 0);
+                                return true;            
+                           // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
+                        }
+
+                        // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
+                        float denom = 1.0f / (va + vb + vc);
+                        v = vb * denom;
+                        w = vc * denom;
+
+                        result.ClosestPointOnSimplex = a + ab * v + ac * w;
+                        result.UsedVertices.UsedVertexA = true;
+                        result.UsedVertices.UsedVertexB = true;
+                        result.UsedVertices.UsedVertexC = true;
+                        result.SetBarycentricCoordinates(1 - v - w, v, w, 0);
+                        
+                        return true;
+                }
+
+                /// Test if point p and d lie on opposite sides of plane through abc
+                public int PointOutsideOfPlane(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d)
+                {
+                        Vector3 normal = Vector3.Cross(b - a, c - a);
+
+                        float signp = Vector3.Dot(p - a, normal); // [AP AB AC]
+                        float signd = Vector3.Dot(d - a, normal); // [AD AB AC]
+
+                        if (CatchDegenerateTetrahedron)
+                                if (signd * signd < (1e-4f * 1e-4f)) return -1;
+
+                        // Points on opposite sides if expression signs are opposite
+                        return signp * signd < 0f ? 1 : 0;
+                }
+                
+                public bool ClosestPtPointTetrahedron(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d,
+                        ref SubSimplexClosestResult finalResult)
+                {
+                        SubSimplexClosestResult tempResult = new SubSimplexClosestResult();
+
+                        // Start out assuming point inside all halfspaces, so closest to itself
+                        finalResult.ClosestPointOnSimplex = p;
+                        finalResult.UsedVertices.Reset();
+                        finalResult.UsedVertices.UsedVertexA = true;
+                        finalResult.UsedVertices.UsedVertexB = true;
+                        finalResult.UsedVertices.UsedVertexC = true;
+                        finalResult.UsedVertices.UsedVertexD = true;
+
+                        int pointOutsideABC = PointOutsideOfPlane(p, a, b, c, d);
+                        int pointOutsideACD = PointOutsideOfPlane(p, a, c, d, b);
+                        int     pointOutsideADB = PointOutsideOfPlane(p, a, d, b, c);
+                        int     pointOutsideBDC = PointOutsideOfPlane(p, b, d, c, a);
+
+                        if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
+                        {
+                                finalResult.Degenerate = true;
+                                return false;
+                        }
+
+                        if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0)
+                                return false;
+
+                        float bestSqDist = float.MaxValue;
+                        // If point outside face abc then compute closest point on abc
+                        if (pointOutsideABC != 0) 
+                        {
+                                ClosestPtPointTriangle(p, a, b, c, ref tempResult);
+                                Vector3 q = tempResult.ClosestPointOnSimplex;
+
+                                float sqDist = ((Vector3)(q - p)).LengthSquared();
+                                // Update best closest point if (squared) distance is less than current best
+                                if (sqDist < bestSqDist)
+                                {
+                                        bestSqDist = sqDist;
+                                        finalResult.ClosestPointOnSimplex = q;
+                                        //convert result bitmask!
+                                        finalResult.UsedVertices.Reset();
+                                        finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA;
+                                        finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexB;
+                                        finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC;
+                                        finalResult.SetBarycentricCoordinates(
+                                                        tempResult.BarycentricCoords[VertexA],
+                                                        tempResult.BarycentricCoords[VertexB],
+                                                        tempResult.BarycentricCoords[VertexC],
+                                                        0);
+                                }
+                        }
+                  
+                        // Repeat test for face acd
+                        if (pointOutsideACD != 0) 
+                        {
+                                ClosestPtPointTriangle(p, a, c, d, ref tempResult);
+                                Vector3 q = tempResult.ClosestPointOnSimplex;
+                                //convert result bitmask!
+
+                                float sqDist = ((Vector3)(q - p)).LengthSquared();
+                                if (sqDist < bestSqDist) 
+                                {
+                                        bestSqDist = sqDist;
+                                        finalResult.ClosestPointOnSimplex = q;
+                                        finalResult.UsedVertices.Reset();
+                                        finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA;
+                                        finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexB;
+                                        finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexC;
+                                        finalResult.SetBarycentricCoordinates(
+                                                        tempResult.BarycentricCoords[VertexA],
+                                                        0,
+                                                        tempResult.BarycentricCoords[VertexB],
+                                                        tempResult.BarycentricCoords[VertexC]);
+                                }
+                        }
+                        // Repeat test for face adb
+                        
+                        if (pointOutsideADB != 0)
+                        {
+                                ClosestPtPointTriangle(p, a, d, b, ref tempResult);
+                                Vector3 q = tempResult.ClosestPointOnSimplex;
+                                //convert result bitmask!
+
+                                float sqDist = ((Vector3)(q - p)).LengthSquared();
+                                if (sqDist < bestSqDist) 
+                                {
+                                        bestSqDist = sqDist;
+                                        finalResult.ClosestPointOnSimplex = q;
+                                        finalResult.UsedVertices.Reset();
+                                        finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA;
+                                        finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB;
+                                        finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexC;
+                                        finalResult.SetBarycentricCoordinates(
+                                                        tempResult.BarycentricCoords[VertexA],
+                                                        tempResult.BarycentricCoords[VertexC],
+                                                        0,
+                                                        tempResult.BarycentricCoords[VertexB]);
+
+                                }
+                        }
+                        // Repeat test for face bdc
+
+                        if (pointOutsideBDC != 0)
+                        {
+                                ClosestPtPointTriangle(p, b, d, c, ref tempResult);
+                                Vector3 q = tempResult.ClosestPointOnSimplex;
+                                //convert result bitmask!
+                                float sqDist = ((Vector3)(q - p)).LengthSquared();
+                                if (sqDist < bestSqDist) 
+                                {
+                                        bestSqDist = sqDist;
+                                        finalResult.ClosestPointOnSimplex = q;
+                                        finalResult.UsedVertices.Reset();
+                                        finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexA;
+                                        finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB;
+                                        finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC;
+
+                                        finalResult.SetBarycentricCoordinates(
+                                                        0,
+                                                        tempResult.BarycentricCoords[VertexA],
+                                                        tempResult.BarycentricCoords[VertexC],
+                                                        tempResult.BarycentricCoords[VertexB]);
+                                }
+                        }
+
+                        //help! we ended up full !
+
+                        if (finalResult.UsedVertices.UsedVertexA &&
+                                finalResult.UsedVertices.UsedVertexB &&
+                                finalResult.UsedVertices.UsedVertexC &&
+                                finalResult.UsedVertices.UsedVertexD) 
+                        {
+                                return true;
+                        }
+
+                        return true;
+                }
+    }
+}