121 files changed, 21376 insertions, 21376 deletions

diff --git a/libraries/ModifiedBulletX/ModifiedBulletX.sln b/libraries/ModifiedBulletX/ModifiedBulletX.sln
index 0eccfdd..79ae566 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX.sln
+++ b/libraries/ModifiedBulletX/ModifiedBulletX.sln
@@ -1,42 +1,42 @@
-

-Microsoft Visual Studio Solution File, Format Version 9.00

-# Visual C# Express 2005

-Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "MonoXnaCompactMaths", "MonoXnaCompactMaths\MonoXnaCompactMaths.csproj", "{121147BC-B06B-406C-84E9-907F268CF0EB}"

-EndProject

-Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Modified.XnaDevRu.BulletX", "ModifiedBulletX\Modified.XnaDevRu.BulletX.csproj", "{44270344-ACA7-4875-B585-81D5C06D0489}"

-EndProject

-Global

-        GlobalSection(SolutionConfigurationPlatforms) = preSolution

-                Debug|Any CPU = Debug|Any CPU

-                Debug|Mixed Platforms = Debug|Mixed Platforms

-                Debug|x86 = Debug|x86

-                Release|Any CPU = Release|Any CPU

-                Release|Mixed Platforms = Release|Mixed Platforms

-                Release|x86 = Release|x86

-        EndGlobalSection

-        GlobalSection(ProjectConfigurationPlatforms) = postSolution

-                {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Any CPU.ActiveCfg = Debug|Any CPU

-                {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Any CPU.Build.0 = Debug|Any CPU

-                {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Mixed Platforms.ActiveCfg = Debug|Any CPU

-                {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Mixed Platforms.Build.0 = Debug|Any CPU

-                {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|x86.ActiveCfg = Debug|Any CPU

-                {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Any CPU.ActiveCfg = Release|Any CPU

-                {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Any CPU.Build.0 = Release|Any CPU

-                {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Mixed Platforms.ActiveCfg = Release|Any CPU

-                {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Mixed Platforms.Build.0 = Release|Any CPU

-                {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|x86.ActiveCfg = Release|Any CPU

-                {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Any CPU.ActiveCfg = Debug|Any CPU

-                {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Any CPU.Build.0 = Debug|Any CPU

-                {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Mixed Platforms.ActiveCfg = Debug|Any CPU

-                {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Mixed Platforms.Build.0 = Debug|Any CPU

-                {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|x86.ActiveCfg = Debug|Any CPU

-                {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Any CPU.ActiveCfg = Release|Any CPU

-                {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Any CPU.Build.0 = Release|Any CPU

-                {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Mixed Platforms.ActiveCfg = Release|Any CPU

-                {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Mixed Platforms.Build.0 = Release|Any CPU

-                {44270344-ACA7-4875-B585-81D5C06D0489}.Release|x86.ActiveCfg = Release|Any CPU

-        EndGlobalSection

-        GlobalSection(SolutionProperties) = preSolution

-                HideSolutionNode = FALSE

-        EndGlobalSection

-EndGlobal

+
+Microsoft Visual Studio Solution File, Format Version 9.00
+# Visual C# Express 2005
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "MonoXnaCompactMaths", "MonoXnaCompactMaths\MonoXnaCompactMaths.csproj", "{121147BC-B06B-406C-84E9-907F268CF0EB}"
+EndProject
+Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Modified.XnaDevRu.BulletX", "ModifiedBulletX\Modified.XnaDevRu.BulletX.csproj", "{44270344-ACA7-4875-B585-81D5C06D0489}"
+EndProject
+Global
+        GlobalSection(SolutionConfigurationPlatforms) = preSolution
+                Debug|Any CPU = Debug|Any CPU
+                Debug|Mixed Platforms = Debug|Mixed Platforms
+                Debug|x86 = Debug|x86
+                Release|Any CPU = Release|Any CPU
+                Release|Mixed Platforms = Release|Mixed Platforms
+                Release|x86 = Release|x86
+        EndGlobalSection
+        GlobalSection(ProjectConfigurationPlatforms) = postSolution
+                {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+                {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Any CPU.Build.0 = Debug|Any CPU
+                {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Mixed Platforms.ActiveCfg = Debug|Any CPU
+                {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Mixed Platforms.Build.0 = Debug|Any CPU
+                {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|x86.ActiveCfg = Debug|Any CPU
+                {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Any CPU.ActiveCfg = Release|Any CPU
+                {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Any CPU.Build.0 = Release|Any CPU
+                {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Mixed Platforms.ActiveCfg = Release|Any CPU
+                {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Mixed Platforms.Build.0 = Release|Any CPU
+                {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|x86.ActiveCfg = Release|Any CPU
+                {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
+                {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Any CPU.Build.0 = Debug|Any CPU
+                {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Mixed Platforms.ActiveCfg = Debug|Any CPU
+                {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Mixed Platforms.Build.0 = Debug|Any CPU
+                {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|x86.ActiveCfg = Debug|Any CPU
+                {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Any CPU.ActiveCfg = Release|Any CPU
+                {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Any CPU.Build.0 = Release|Any CPU
+                {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Mixed Platforms.ActiveCfg = Release|Any CPU
+                {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Mixed Platforms.Build.0 = Release|Any CPU
+                {44270344-ACA7-4875-B585-81D5C06D0489}.Release|x86.ActiveCfg = Release|Any CPU
+        EndGlobalSection
+        GlobalSection(SolutionProperties) = preSolution
+                HideSolutionNode = FALSE
+        EndGlobalSection
+EndGlobal
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs b/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs
index d0d5845..a4cbfdd 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs
@@ -1,68 +1,68 @@
-/*

-  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 System.Diagnostics;

-

-namespace XnaDevRu.BulletX

-{

-        internal static class BulletDebug

-        {

-                [Conditional("DEBUG")]

-                public static void Assert(Boolean condition)

-                {

-                        //if (!condition)

-                        //{

-                        //    Throw("No info available");

-                        //}

-                        Debug.Assert(condition);

-                }

-

-                [Conditional("DEBUG")]

-                public static void Assert(Boolean condition, String message)

-                {

-                        //if (!condition)

-                        //{

-                        //    Throw(message);

-                        //}

-                        Debug.Assert(condition, message);

-                }

-

-                [Conditional("DEBUG")]

-                public static void Assert(Boolean condition, String message, String detailMessage)

-                {

-                        //if (!condition)

-                        //{

-                        //    Throw(message);

-                        //}

-                        Debug.Assert(condition, message, detailMessage);

-                }

-

-                private static void Throw(String message)

-                {

-                        String msg = String.Format("Assertion Error: {0}", message);

-

-                        throw new BulletException(msg);

-                }

-        }

-}

+/*
+  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 System.Diagnostics;
+
+namespace XnaDevRu.BulletX
+{
+        internal static class BulletDebug
+        {
+                [Conditional("DEBUG")]
+                public static void Assert(Boolean condition)
+                {
+                        //if (!condition)
+                        //{
+                        //    Throw("No info available");
+                        //}
+                        Debug.Assert(condition);
+                }
+
+                [Conditional("DEBUG")]
+                public static void Assert(Boolean condition, String message)
+                {
+                        //if (!condition)
+                        //{
+                        //    Throw(message);
+                        //}
+                        Debug.Assert(condition, message);
+                }
+
+                [Conditional("DEBUG")]
+                public static void Assert(Boolean condition, String message, String detailMessage)
+                {
+                        //if (!condition)
+                        //{
+                        //    Throw(message);
+                        //}
+                        Debug.Assert(condition, message, detailMessage);
+                }
+
+                private static void Throw(String message)
+                {
+                        String msg = String.Format("Assertion Error: {0}", message);
+
+                        throw new BulletException(msg);
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs
index 12692ea..168d947 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs
@@ -1,623 +1,623 @@
-/*

-  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 AxisSweep3: OverlappingPairCache

-    {

-        Vector3 _worldAabbMin;

-        Vector3 _worldAabbMax;

-

-        Vector3 _quantize;

-

-        int _numHandles;

-        int _maxHandles;

-

-        Handle[] _handles;

-        Edge[][] _edges = new Edge[3][];

-

-        ushort _firstFreeHandle;

-

-                int _invalidPair;

-

-        public AxisSweep3(Vector3 worldAabbMin, Vector3 worldAabbMax, int maxHandles)

-            : base()

-        {

-            BulletDebug.Assert(maxHandles > 1 && maxHandles < 32767);

-

-            // init bounds

-            _worldAabbMin = worldAabbMin;

-            _worldAabbMax = worldAabbMax;

-

-            Vector3 aabbSize = _worldAabbMax - _worldAabbMin;

-            _quantize = new Vector3(65535.0f, 65535.0f, 65535.0f) / aabbSize;

-

-            // allocate handles buffer and put all handles on free list

-            _handles = new Handle[maxHandles];

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

-                _handles[i] = new Handle();

-            _maxHandles = maxHandles;

-            _numHandles = 0;

-

-            // handle 0 is reserved as the null index, and is also used as the sentinel

-            _firstFreeHandle = 1;

-            {

-                for (int i = _firstFreeHandle; i < maxHandles; i++)

-                {

-                    _handles[i].NextFree = (ushort)(i + 1);

-                }

-                _handles[maxHandles - 1].NextFree = 0;

-            }

-

-            {

-                // allocate edge buffers

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

-                {

-                    _edges[i] = new Edge[maxHandles * 2];

-                    for (int j = 0; j < maxHandles * 2; j++)

-                    {

-                        _edges[i][j] = new Edge();

-                    }

-                }

-            }

-            //removed overlap management

-

-            // make boundary sentinels

-

-            _handles[0].ClientData = 0;

-

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

-            {

-                _handles[0].MinEdges[axis] = 0;

-                _handles[0].MaxEdges[axis] = 1;

-

-                _edges[axis][0].Position = 0;

-                _edges[axis][0].Handle = 0;

-                _edges[axis][1].Position = 0xffff;

-                _edges[axis][1].Handle = 0;

-            }

-        }

-

-        public ushort AddHandle(Vector3 aabbMin, Vector3 aabbMax, object owner, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask)

-        {

-            ushort[] min = new ushort[3], max = new ushort[3];

-            Quantize(out min, aabbMin, 0);

-            Quantize(out max, aabbMax, 1);

-

-            ushort handle = AllocateHandle();

-            Handle oHandle = GetHandle(handle);

-

-            oHandle.HandleID = handle;

-            oHandle.ClientData = owner;

-            oHandle.CollisionFilterGroup = collisionFilterGroup;

-            oHandle.CollisionFilterMask = collisionFilterMask;

-

-            int limit = _numHandles * 2;

-

-            // (Gluk    )

-            //  ( Inside )

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

-            {

-                _handles[0].MaxEdges[axis] += 2;

-

-                _edges[axis][limit + 1].Position = _edges[axis][limit - 1].Position;

-                _edges[axis][limit + 1].Handle = _edges[axis][limit - 1].Handle;

-

-                _edges[axis][limit - 1].Position = min[axis];

-                _edges[axis][limit - 1].Handle = handle;

-

-                _edges[axis][limit].Position = max[axis];

-                _edges[axis][limit].Handle = handle;

-

-                oHandle.MinEdges[axis] = (ushort)(limit - 1);

-                oHandle.MaxEdges[axis] = (ushort)limit;

-            }

-

-            SortMinDown(0, oHandle.MinEdges[0], false);

-            SortMaxDown(0, oHandle.MaxEdges[0], false);

-            SortMinDown(1, oHandle.MinEdges[1], false);

-            SortMaxDown(1, oHandle.MaxEdges[1], false);

-            SortMinDown(2, oHandle.MinEdges[2], true);

-            SortMaxDown(2, oHandle.MaxEdges[2], true);

-

-            return handle;

-        }

-

-        public void RemoveHandle(ushort handle)

-        {

-            Handle pHandle = GetHandle(handle);

-

-            //explicitly remove the pairs containing the proxy

-            //we could do it also in the sortMinUp (passing true)

-            //todo: compare performance

-            RemoveOverlappingPairsContainingProxy(pHandle);

-

-

-            // compute current limit of edge arrays

-            int limit = _numHandles * 2;

-            int axis;

-

-            for (axis = 0; axis < 3; axis++)

-            {

-                _handles[0].MaxEdges[axis] -= 2;

-            }

-

-            // remove the edges by sorting them up to the end of the list

-            for (axis = 0; axis < 3; axis++)

-            {

-                Edge[] pEdges = _edges[axis];

-                ushort max = pHandle.MaxEdges[axis];

-                pEdges[max].Position = 0xffff;

-

-                SortMaxUp(axis, max, false);

-

-                ushort i = pHandle.MinEdges[axis];

-                pEdges[i].Position = 0xffff;

-

-                SortMinUp(axis, i, false);

-

-                pEdges[limit - 1].Handle = 0;

-                pEdges[limit - 1].Position = 0xffff;

-            }

-

-            // free the handle

-            FreeHandle(handle);            

-        }

-

-                public override void ProcessAllOverlappingPairs(IOverlapCallback callback)

-                {

-                        OverlappingPairs.Sort(new Comparison<BroadphasePair>(BroadphasePair.ComparisonSort));

-

-                        if (_invalidPair != 0)

-                                OverlappingPairs.RemoveRange(OverlappingPairs.Count - _invalidPair, _invalidPair);

-                        _invalidPair = 0;

-

-                        BroadphasePair previousPair = new BroadphasePair();

-                        previousPair.ProxyA = null;

-                        previousPair.ProxyB = null;

-                        previousPair.CollisionAlgorithm = null;

-

-                        List<BroadphasePair> removal = new List<BroadphasePair>();

-

-                        for (int i = 0; i < OverlappingPairs.Count; i++)

-                        {

-                                bool isDuplicate = (OverlappingPairs[i] == previousPair);

-                                previousPair = OverlappingPairs[i];

-                                bool needsRemoval;

-                                if (!isDuplicate)

-                                {

-                                        bool hasOverlap = TestOverlap(previousPair.ProxyA, previousPair.ProxyB);

-                                        if (hasOverlap)

-                                        {

-                                                needsRemoval = callback.ProcessOverlap(ref previousPair);

-                                        }

-                                        else

-                                        {

-                                                needsRemoval = true;

-                                        }

-                                }

-                                else

-                                {

-                                        needsRemoval = true;

-                                        BulletDebug.Assert(previousPair.CollisionAlgorithm == null);

-                                }

-

-                                if (needsRemoval)

-                                {

-                                        removal.Add(previousPair);

-                                }

-                        }

-

-                        for (int i = 0; i < removal.Count; i++)

-                        {

-                                BroadphasePair pair = removal[i];

-                                CleanOverlappingPair(ref pair);

-                                pair.ProxyA = null;

-                                pair.ProxyB = null;

-                                _invalidPair++;

-                                OverlappingPairCount--;

-                        }

-                }

-

-                private bool TestOverlap(BroadphaseProxy proxyA, BroadphaseProxy proxyB)

-                {

-                        if (proxyA == null || proxyB == null)

-                                return false;

-

-                        Handle handleA = proxyA as Handle;

-                        Handle handleB = proxyB as Handle;

-

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

-                        {

-                                if (handleA.MaxEdges[axis] < handleB.MinEdges[axis] ||

-                                        handleB.MaxEdges[axis] < handleA.MinEdges[axis])

-                                {

-                                        return false;

-                                }

-                        }

-                        return true;

-                }

-

-                private bool TestOverlap(int ignoreAxis, Handle pHandleA, Handle pHandleB)

-        {

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

-                { 

-                        if (axis != ignoreAxis)

-                        {

-                    if (pHandleA.MaxEdges[axis] < pHandleB.MinEdges[axis] ||

-                        pHandleB.MaxEdges[axis] < pHandleA.MinEdges[axis]) 

-                                { 

-                                        return false; 

-                                } 

-                        }

-                } 

-

-                return true;

-        }

-

-        private ushort AllocateHandle() 

-        {

-            ushort handle = _firstFreeHandle;

-            _firstFreeHandle = GetHandle(handle).NextFree;

-            _numHandles++;

-

-            return handle;

-        }

-

-        private void FreeHandle(ushort handle)

-        {

-            BulletDebug.Assert(handle > 0 && handle < _maxHandles);

-

-            GetHandle(handle).NextFree = _firstFreeHandle;

-            _firstFreeHandle = handle;

-

-            _numHandles--;            

-        }

-

-        private Handle GetHandle(ushort handle)

-        {

-            return _handles[handle];

-        }

-

-        private void UpdateHandle(ushort handle, Vector3 aabbMin, Vector3 aabbMax)

-        {

-                Handle pHandle = GetHandle(handle);

-

-                // quantize the new bounds

-                ushort[] min = new ushort[3];

-            ushort[] max = new ushort[3];

-                Quantize(out min, aabbMin, 0);

-                Quantize(out max, aabbMax, 1);

-

-                // update changed edges

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

-                {

-                        ushort emin = pHandle.MinEdges[axis];

-                ushort emax = pHandle.MaxEdges[axis];

-

-                        int dmin = (int)min[axis] - (int)_edges[axis][emin].Position;

-                        int dmax = (int)max[axis] - (int)_edges[axis][emax].Position;

-

-                        _edges[axis][emin].Position = min[axis];

-                _edges[axis][emax].Position = max[axis];

-

-                        // expand (only adds overlaps)

-                        if (dmin < 0)

-                                SortMinDown(axis, emin, true);

-

-                        if (dmax > 0)

-                    SortMaxUp(axis, emax, true);

-

-                        // shrink (only removes overlaps)

-                        if (dmin > 0)

-                    SortMinUp(axis, emin, true);

-

-                        if (dmax < 0)

-                    SortMaxDown(axis, emax, true);

-                }

-        }

-

-        private void Quantize(out ushort[] result, Vector3 point, int isMax)

-        {

-            Vector3 clampedPoint = new Vector3(

-                point.X,

-                point.Y,

-                point.Z

-            );

-

-            MathHelper.SetMax(ref clampedPoint, _worldAabbMin);

-            MathHelper.SetMin(ref clampedPoint, _worldAabbMax);

-

-            Vector3 v = (clampedPoint - _worldAabbMin) * _quantize;

-

-            result = new ushort[3];

-            result[0] = (ushort)(((int)v.X & 0xfffe) | isMax);

-            result[1] = (ushort)(((int)v.Y & 0xfffe) | isMax);

-            result[2] = (ushort)(((int)v.Z & 0xfffe) | isMax);

-        }

-

-        private void SortMinDown(int axis, ushort edge, bool updateOverlaps)

-        {

-            Edge pEdge = _edges[axis][edge];

-            Edge pPrev = _edges[axis][edge - 1];

-            Handle pHandleEdge = GetHandle(pEdge.Handle);

-

-            while (pEdge.Position < pPrev.Position)

-            {

-                Handle pHandlePrev = GetHandle(pPrev.Handle);

-

-                if (pPrev.IsMax())

-                {

-                    // if previous edge is a maximum check the bounds and add an overlap if necessary

-                    if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandlePrev))

-                    {

-                        AddOverlappingPair(pHandleEdge, pHandlePrev);

-                    }

-

-                    // update edge reference in other handle

-                    pHandlePrev.MaxEdges[axis]++;

-                }

-                else

-                    pHandlePrev.MinEdges[axis]++;

-

-                pHandleEdge.MinEdges[axis]--;

-

-                // swap the edges

-                pEdge.Swap(ref pPrev);

-

-                // decrement

-                edge--;

-                pEdge = _edges[axis][edge];

-                pPrev = _edges[axis][edge - 1];

-            }

-        }

-

-        private void SortMinUp(int axis, ushort edge, bool updateOverlaps)

-        {

-            Edge pEdge = _edges[axis][edge];

-            Edge pNext = _edges[axis][edge + 1];

-                Handle pHandleEdge = GetHandle(pEdge.Handle);

-

-            while ((pNext.Handle != 0) && (pEdge.Position >= pNext.Position))

-                {

-                        Handle pHandleNext = GetHandle(pNext.Handle);

-

-                        if (pNext.IsMax())

-                        {

-                                // if next edge is maximum remove any overlap between the two handles

-                                if (updateOverlaps)

-                                {

-                                                //Handle handle0 = GetHandle(pEdge.Handle);

-                                                //Handle handle1 = GetHandle(pNext.Handle);

-                                                //BroadphasePair tmpPair = new BroadphasePair(handle0, handle1);

-                                                //RemoveOverlappingPair(tmpPair);

-                                }

-

-                                // update edge reference in other handle

-                                pHandleNext.MaxEdges[axis]--;

-                        }

-                        else

-                    pHandleNext.MinEdges[axis]--;

-

-                pHandleEdge.MinEdges[axis]++;

-

-                // swap the edges

-                pEdge.Swap(ref pNext);

-

-                // increment

-                edge++;

-                pEdge = _edges[axis][edge];

-                pNext = _edges[axis][edge + 1];

-                }

-        }

-

-        private void SortMaxDown(int axis, ushort edge, bool updateOverlaps)

-        {

-            Edge pEdge = _edges[axis][edge];

-            Edge pPrev = _edges[axis][edge - 1];

-            Handle pHandleEdge = GetHandle(pEdge.Handle);

-

-            while (pEdge.Position < pPrev.Position)

-            {

-                Handle pHandlePrev = GetHandle(pPrev.Handle);

-

-                if (!pPrev.IsMax())

-                {

-                    // if previous edge was a minimum remove any overlap between the two handles

-                    if (updateOverlaps)

-                    {

-                                                //this is done during the overlappingpairarray iteration/narrowphase collision

-                                                //Handle handle0 = GetHandle(pEdge.Handle);

-                                                //Handle handle1 = GetHandle(pPrev.Handle);

-                                                //BroadphasePair pair = FindPair(handle0, handle1);

-

-                                                //if (pair != null)

-                                                //{

-                                                //    RemoveOverlappingPair(pair);

-                                                //}

-                    }

-

-                    // update edge reference in other handle

-                    pHandlePrev.MinEdges[axis]++; ;

-                }

-                else

-                    pHandlePrev.MaxEdges[axis]++;

-

-                pHandleEdge.MaxEdges[axis]--;

-

-                // swap the edges

-                pEdge.Swap(ref pPrev);

-

-                // decrement

-                edge--;

-                pEdge = _edges[axis][edge];

-                pPrev = _edges[axis][edge - 1];

-            }

-        }

-

-        private void SortMaxUp(int axis, ushort edge, bool updateOverlaps) 

-        {

-            Edge pEdge = _edges[axis][edge];

-            Edge pNext = _edges[axis][edge + 1];

-            Handle pHandleEdge = GetHandle(pEdge.Handle);

-

-            while ((pNext.Handle!=0) && (pEdge.Position >= pNext.Position))

-            {

-                Handle pHandleNext = GetHandle(pNext.Handle);

-

-                if (!pNext.IsMax())

-                {

-                    // if next edge is a minimum check the bounds and add an overlap if necessary

-                    if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandleNext))

-                    {

-                        Handle handle0 = GetHandle(pEdge.Handle);

-                        Handle handle1 = GetHandle(pNext.Handle);

-                        AddOverlappingPair(handle0, handle1);

-                    }

-

-                    // update edge reference in other handle

-                    pHandleNext.MinEdges[axis]--;

-                }

-                else

-                    pHandleNext.MaxEdges[axis]--;

-

-                pHandleEdge.MaxEdges[axis]++;

-

-                // swap the edges

-                pEdge.Swap(ref pNext);

-

-                // increment

-                edge++;

-                pEdge = _edges[axis][edge];

-                pNext = _edges[axis][edge + 1];

-            }

-        }

-

-        #region Abstract

-

-        public override void RefreshOverlappingPairs()

-        {

-        }

-

-        public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask)

-        {

-                    ushort handleId = AddHandle(min, max, userData, collisionFilterGroup, collisionFilterMask);

-                

-                    Handle handle = GetHandle(handleId);

-                                

-                    return handle;

-        }

-

-        public override void DestroyProxy(BroadphaseProxy proxy)

-        {

-            Handle handle = proxy as Handle;

-            RemoveHandle(handle.HandleID);

-        }

-

-        public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax)

-        {

-            Handle handle = proxy as Handle;

-            UpdateHandle(handle.HandleID, aabbMin, aabbMax);

-        }

-        #endregion

-    }

-

-    public class Edge

-    {

-        ushort position;

-        ushort handle;

-

-        public ushort Position

-        {

-            get { return position; }

-            set { position = value; }

-        }

-

-        public ushort Handle

-        {

-            get { return handle; }

-            set { handle = value; }

-        }

-

-        public bool IsMax()

-        {

-            return (position & (ushort)1) == 1;

-        }

-

-        public void Swap(ref Edge e)

-        {

-            ushort tmpPosition = this.position;

-            ushort tmpHandle = this.handle;

-            this.position = e.position;

-            this.handle = e.handle;

-            e.position = tmpPosition;

-            e.handle = tmpHandle;

-        }

-    }

-

-    public class Handle: BroadphaseProxy

-    {

-        ushort[] minEdges, maxEdges;

-        ushort pad;

-        ushort handleID;

-

-        public ushort[] MinEdges

-        {

-            get { return minEdges; }

-            set { minEdges = value; }

-        }

-

-        public ushort[] MaxEdges

-        {

-            get { return maxEdges; }

-            set { maxEdges = value; }

-        }

-

-        public ushort HandleID

-        {

-            get { return handleID; }

-            set { handleID = value; }

-        }

-

-        public ushort Pad

-        {

-            get { return pad; }

-            set { pad = value; }

-        }

-

-        public ushort NextFree

-        {

-            get { return minEdges[0]; }

-            set { minEdges[0] = value;}

-        }

-

-        public Handle()

-        {

-            minEdges = new ushort[3];

-            maxEdges = new ushort[3];

-        }

-    }

-}

+/*
+  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 AxisSweep3: OverlappingPairCache
+    {
+        Vector3 _worldAabbMin;
+        Vector3 _worldAabbMax;
+
+        Vector3 _quantize;
+
+        int _numHandles;
+        int _maxHandles;
+
+        Handle[] _handles;
+        Edge[][] _edges = new Edge[3][];
+
+        ushort _firstFreeHandle;
+
+                int _invalidPair;
+
+        public AxisSweep3(Vector3 worldAabbMin, Vector3 worldAabbMax, int maxHandles)
+            : base()
+        {
+            BulletDebug.Assert(maxHandles > 1 && maxHandles < 32767);
+
+            // init bounds
+            _worldAabbMin = worldAabbMin;
+            _worldAabbMax = worldAabbMax;
+
+            Vector3 aabbSize = _worldAabbMax - _worldAabbMin;
+            _quantize = new Vector3(65535.0f, 65535.0f, 65535.0f) / aabbSize;
+
+            // allocate handles buffer and put all handles on free list
+            _handles = new Handle[maxHandles];
+            for (int i = 0; i < maxHandles; i++)
+                _handles[i] = new Handle();
+            _maxHandles = maxHandles;
+            _numHandles = 0;
+
+            // handle 0 is reserved as the null index, and is also used as the sentinel
+            _firstFreeHandle = 1;
+            {
+                for (int i = _firstFreeHandle; i < maxHandles; i++)
+                {
+                    _handles[i].NextFree = (ushort)(i + 1);
+                }
+                _handles[maxHandles - 1].NextFree = 0;
+            }
+
+            {
+                // allocate edge buffers
+                for (int i = 0; i < 3; i++)
+                {
+                    _edges[i] = new Edge[maxHandles * 2];
+                    for (int j = 0; j < maxHandles * 2; j++)
+                    {
+                        _edges[i][j] = new Edge();
+                    }
+                }
+            }
+            //removed overlap management
+
+            // make boundary sentinels
+
+            _handles[0].ClientData = 0;
+
+            for (int axis = 0; axis < 3; axis++)
+            {
+                _handles[0].MinEdges[axis] = 0;
+                _handles[0].MaxEdges[axis] = 1;
+
+                _edges[axis][0].Position = 0;
+                _edges[axis][0].Handle = 0;
+                _edges[axis][1].Position = 0xffff;
+                _edges[axis][1].Handle = 0;
+            }
+        }
+
+        public ushort AddHandle(Vector3 aabbMin, Vector3 aabbMax, object owner, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask)
+        {
+            ushort[] min = new ushort[3], max = new ushort[3];
+            Quantize(out min, aabbMin, 0);
+            Quantize(out max, aabbMax, 1);
+
+            ushort handle = AllocateHandle();
+            Handle oHandle = GetHandle(handle);
+
+            oHandle.HandleID = handle;
+            oHandle.ClientData = owner;
+            oHandle.CollisionFilterGroup = collisionFilterGroup;
+            oHandle.CollisionFilterMask = collisionFilterMask;
+
+            int limit = _numHandles * 2;
+
+            // (Gluk    )
+            //  ( Inside )
+            for (int axis = 0; axis < 3; axis++)
+            {
+                _handles[0].MaxEdges[axis] += 2;
+
+                _edges[axis][limit + 1].Position = _edges[axis][limit - 1].Position;
+                _edges[axis][limit + 1].Handle = _edges[axis][limit - 1].Handle;
+
+                _edges[axis][limit - 1].Position = min[axis];
+                _edges[axis][limit - 1].Handle = handle;
+
+                _edges[axis][limit].Position = max[axis];
+                _edges[axis][limit].Handle = handle;
+
+                oHandle.MinEdges[axis] = (ushort)(limit - 1);
+                oHandle.MaxEdges[axis] = (ushort)limit;
+            }
+
+            SortMinDown(0, oHandle.MinEdges[0], false);
+            SortMaxDown(0, oHandle.MaxEdges[0], false);
+            SortMinDown(1, oHandle.MinEdges[1], false);
+            SortMaxDown(1, oHandle.MaxEdges[1], false);
+            SortMinDown(2, oHandle.MinEdges[2], true);
+            SortMaxDown(2, oHandle.MaxEdges[2], true);
+
+            return handle;
+        }
+
+        public void RemoveHandle(ushort handle)
+        {
+            Handle pHandle = GetHandle(handle);
+
+            //explicitly remove the pairs containing the proxy
+            //we could do it also in the sortMinUp (passing true)
+            //todo: compare performance
+            RemoveOverlappingPairsContainingProxy(pHandle);
+
+
+            // compute current limit of edge arrays
+            int limit = _numHandles * 2;
+            int axis;
+
+            for (axis = 0; axis < 3; axis++)
+            {
+                _handles[0].MaxEdges[axis] -= 2;
+            }
+
+            // remove the edges by sorting them up to the end of the list
+            for (axis = 0; axis < 3; axis++)
+            {
+                Edge[] pEdges = _edges[axis];
+                ushort max = pHandle.MaxEdges[axis];
+                pEdges[max].Position = 0xffff;
+
+                SortMaxUp(axis, max, false);
+
+                ushort i = pHandle.MinEdges[axis];
+                pEdges[i].Position = 0xffff;
+
+                SortMinUp(axis, i, false);
+
+                pEdges[limit - 1].Handle = 0;
+                pEdges[limit - 1].Position = 0xffff;
+            }
+
+            // free the handle
+            FreeHandle(handle);            
+        }
+
+                public override void ProcessAllOverlappingPairs(IOverlapCallback callback)
+                {
+                        OverlappingPairs.Sort(new Comparison<BroadphasePair>(BroadphasePair.ComparisonSort));
+
+                        if (_invalidPair != 0)
+                                OverlappingPairs.RemoveRange(OverlappingPairs.Count - _invalidPair, _invalidPair);
+                        _invalidPair = 0;
+
+                        BroadphasePair previousPair = new BroadphasePair();
+                        previousPair.ProxyA = null;
+                        previousPair.ProxyB = null;
+                        previousPair.CollisionAlgorithm = null;
+
+                        List<BroadphasePair> removal = new List<BroadphasePair>();
+
+                        for (int i = 0; i < OverlappingPairs.Count; i++)
+                        {
+                                bool isDuplicate = (OverlappingPairs[i] == previousPair);
+                                previousPair = OverlappingPairs[i];
+                                bool needsRemoval;
+                                if (!isDuplicate)
+                                {
+                                        bool hasOverlap = TestOverlap(previousPair.ProxyA, previousPair.ProxyB);
+                                        if (hasOverlap)
+                                        {
+                                                needsRemoval = callback.ProcessOverlap(ref previousPair);
+                                        }
+                                        else
+                                        {
+                                                needsRemoval = true;
+                                        }
+                                }
+                                else
+                                {
+                                        needsRemoval = true;
+                                        BulletDebug.Assert(previousPair.CollisionAlgorithm == null);
+                                }
+
+                                if (needsRemoval)
+                                {
+                                        removal.Add(previousPair);
+                                }
+                        }
+
+                        for (int i = 0; i < removal.Count; i++)
+                        {
+                                BroadphasePair pair = removal[i];
+                                CleanOverlappingPair(ref pair);
+                                pair.ProxyA = null;
+                                pair.ProxyB = null;
+                                _invalidPair++;
+                                OverlappingPairCount--;
+                        }
+                }
+
+                private bool TestOverlap(BroadphaseProxy proxyA, BroadphaseProxy proxyB)
+                {
+                        if (proxyA == null || proxyB == null)
+                                return false;
+
+                        Handle handleA = proxyA as Handle;
+                        Handle handleB = proxyB as Handle;
+
+                        for (int axis = 0; axis < 3; axis++)
+                        {
+                                if (handleA.MaxEdges[axis] < handleB.MinEdges[axis] ||
+                                        handleB.MaxEdges[axis] < handleA.MinEdges[axis])
+                                {
+                                        return false;
+                                }
+                        }
+                        return true;
+                }
+
+                private bool TestOverlap(int ignoreAxis, Handle pHandleA, Handle pHandleB)
+        {
+                for (int axis = 0; axis < 3; axis++)
+                { 
+                        if (axis != ignoreAxis)
+                        {
+                    if (pHandleA.MaxEdges[axis] < pHandleB.MinEdges[axis] ||
+                        pHandleB.MaxEdges[axis] < pHandleA.MinEdges[axis]) 
+                                { 
+                                        return false; 
+                                } 
+                        }
+                } 
+
+                return true;
+        }
+
+        private ushort AllocateHandle() 
+        {
+            ushort handle = _firstFreeHandle;
+            _firstFreeHandle = GetHandle(handle).NextFree;
+            _numHandles++;
+
+            return handle;
+        }
+
+        private void FreeHandle(ushort handle)
+        {
+            BulletDebug.Assert(handle > 0 && handle < _maxHandles);
+
+            GetHandle(handle).NextFree = _firstFreeHandle;
+            _firstFreeHandle = handle;
+
+            _numHandles--;            
+        }
+
+        private Handle GetHandle(ushort handle)
+        {
+            return _handles[handle];
+        }
+
+        private void UpdateHandle(ushort handle, Vector3 aabbMin, Vector3 aabbMax)
+        {
+                Handle pHandle = GetHandle(handle);
+
+                // quantize the new bounds
+                ushort[] min = new ushort[3];
+            ushort[] max = new ushort[3];
+                Quantize(out min, aabbMin, 0);
+                Quantize(out max, aabbMax, 1);
+
+                // update changed edges
+                for (int axis = 0; axis < 3; axis++)
+                {
+                        ushort emin = pHandle.MinEdges[axis];
+                ushort emax = pHandle.MaxEdges[axis];
+
+                        int dmin = (int)min[axis] - (int)_edges[axis][emin].Position;
+                        int dmax = (int)max[axis] - (int)_edges[axis][emax].Position;
+
+                        _edges[axis][emin].Position = min[axis];
+                _edges[axis][emax].Position = max[axis];
+
+                        // expand (only adds overlaps)
+                        if (dmin < 0)
+                                SortMinDown(axis, emin, true);
+
+                        if (dmax > 0)
+                    SortMaxUp(axis, emax, true);
+
+                        // shrink (only removes overlaps)
+                        if (dmin > 0)
+                    SortMinUp(axis, emin, true);
+
+                        if (dmax < 0)
+                    SortMaxDown(axis, emax, true);
+                }
+        }
+
+        private void Quantize(out ushort[] result, Vector3 point, int isMax)
+        {
+            Vector3 clampedPoint = new Vector3(
+                point.X,
+                point.Y,
+                point.Z
+            );
+
+            MathHelper.SetMax(ref clampedPoint, _worldAabbMin);
+            MathHelper.SetMin(ref clampedPoint, _worldAabbMax);
+
+            Vector3 v = (clampedPoint - _worldAabbMin) * _quantize;
+
+            result = new ushort[3];
+            result[0] = (ushort)(((int)v.X & 0xfffe) | isMax);
+            result[1] = (ushort)(((int)v.Y & 0xfffe) | isMax);
+            result[2] = (ushort)(((int)v.Z & 0xfffe) | isMax);
+        }
+
+        private void SortMinDown(int axis, ushort edge, bool updateOverlaps)
+        {
+            Edge pEdge = _edges[axis][edge];
+            Edge pPrev = _edges[axis][edge - 1];
+            Handle pHandleEdge = GetHandle(pEdge.Handle);
+
+            while (pEdge.Position < pPrev.Position)
+            {
+                Handle pHandlePrev = GetHandle(pPrev.Handle);
+
+                if (pPrev.IsMax())
+                {
+                    // if previous edge is a maximum check the bounds and add an overlap if necessary
+                    if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandlePrev))
+                    {
+                        AddOverlappingPair(pHandleEdge, pHandlePrev);
+                    }
+
+                    // update edge reference in other handle
+                    pHandlePrev.MaxEdges[axis]++;
+                }
+                else
+                    pHandlePrev.MinEdges[axis]++;
+
+                pHandleEdge.MinEdges[axis]--;
+
+                // swap the edges
+                pEdge.Swap(ref pPrev);
+
+                // decrement
+                edge--;
+                pEdge = _edges[axis][edge];
+                pPrev = _edges[axis][edge - 1];
+            }
+        }
+
+        private void SortMinUp(int axis, ushort edge, bool updateOverlaps)
+        {
+            Edge pEdge = _edges[axis][edge];
+            Edge pNext = _edges[axis][edge + 1];
+                Handle pHandleEdge = GetHandle(pEdge.Handle);
+
+            while ((pNext.Handle != 0) && (pEdge.Position >= pNext.Position))
+                {
+                        Handle pHandleNext = GetHandle(pNext.Handle);
+
+                        if (pNext.IsMax())
+                        {
+                                // if next edge is maximum remove any overlap between the two handles
+                                if (updateOverlaps)
+                                {
+                                                //Handle handle0 = GetHandle(pEdge.Handle);
+                                                //Handle handle1 = GetHandle(pNext.Handle);
+                                                //BroadphasePair tmpPair = new BroadphasePair(handle0, handle1);
+                                                //RemoveOverlappingPair(tmpPair);
+                                }
+
+                                // update edge reference in other handle
+                                pHandleNext.MaxEdges[axis]--;
+                        }
+                        else
+                    pHandleNext.MinEdges[axis]--;
+
+                pHandleEdge.MinEdges[axis]++;
+
+                // swap the edges
+                pEdge.Swap(ref pNext);
+
+                // increment
+                edge++;
+                pEdge = _edges[axis][edge];
+                pNext = _edges[axis][edge + 1];
+                }
+        }
+
+        private void SortMaxDown(int axis, ushort edge, bool updateOverlaps)
+        {
+            Edge pEdge = _edges[axis][edge];
+            Edge pPrev = _edges[axis][edge - 1];
+            Handle pHandleEdge = GetHandle(pEdge.Handle);
+
+            while (pEdge.Position < pPrev.Position)
+            {
+                Handle pHandlePrev = GetHandle(pPrev.Handle);
+
+                if (!pPrev.IsMax())
+                {
+                    // if previous edge was a minimum remove any overlap between the two handles
+                    if (updateOverlaps)
+                    {
+                                                //this is done during the overlappingpairarray iteration/narrowphase collision
+                                                //Handle handle0 = GetHandle(pEdge.Handle);
+                                                //Handle handle1 = GetHandle(pPrev.Handle);
+                                                //BroadphasePair pair = FindPair(handle0, handle1);
+
+                                                //if (pair != null)
+                                                //{
+                                                //    RemoveOverlappingPair(pair);
+                                                //}
+                    }
+
+                    // update edge reference in other handle
+                    pHandlePrev.MinEdges[axis]++; ;
+                }
+                else
+                    pHandlePrev.MaxEdges[axis]++;
+
+                pHandleEdge.MaxEdges[axis]--;
+
+                // swap the edges
+                pEdge.Swap(ref pPrev);
+
+                // decrement
+                edge--;
+                pEdge = _edges[axis][edge];
+                pPrev = _edges[axis][edge - 1];
+            }
+        }
+
+        private void SortMaxUp(int axis, ushort edge, bool updateOverlaps) 
+        {
+            Edge pEdge = _edges[axis][edge];
+            Edge pNext = _edges[axis][edge + 1];
+            Handle pHandleEdge = GetHandle(pEdge.Handle);
+
+            while ((pNext.Handle!=0) && (pEdge.Position >= pNext.Position))
+            {
+                Handle pHandleNext = GetHandle(pNext.Handle);
+
+                if (!pNext.IsMax())
+                {
+                    // if next edge is a minimum check the bounds and add an overlap if necessary
+                    if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandleNext))
+                    {
+                        Handle handle0 = GetHandle(pEdge.Handle);
+                        Handle handle1 = GetHandle(pNext.Handle);
+                        AddOverlappingPair(handle0, handle1);
+                    }
+
+                    // update edge reference in other handle
+                    pHandleNext.MinEdges[axis]--;
+                }
+                else
+                    pHandleNext.MaxEdges[axis]--;
+
+                pHandleEdge.MaxEdges[axis]++;
+
+                // swap the edges
+                pEdge.Swap(ref pNext);
+
+                // increment
+                edge++;
+                pEdge = _edges[axis][edge];
+                pNext = _edges[axis][edge + 1];
+            }
+        }
+
+        #region Abstract
+
+        public override void RefreshOverlappingPairs()
+        {
+        }
+
+        public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask)
+        {
+                    ushort handleId = AddHandle(min, max, userData, collisionFilterGroup, collisionFilterMask);
+                
+                    Handle handle = GetHandle(handleId);
+                                
+                    return handle;
+        }
+
+        public override void DestroyProxy(BroadphaseProxy proxy)
+        {
+            Handle handle = proxy as Handle;
+            RemoveHandle(handle.HandleID);
+        }
+
+        public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax)
+        {
+            Handle handle = proxy as Handle;
+            UpdateHandle(handle.HandleID, aabbMin, aabbMax);
+        }
+        #endregion
+    }
+
+    public class Edge
+    {
+        ushort position;
+        ushort handle;
+
+        public ushort Position
+        {
+            get { return position; }
+            set { position = value; }
+        }
+
+        public ushort Handle
+        {
+            get { return handle; }
+            set { handle = value; }
+        }
+
+        public bool IsMax()
+        {
+            return (position & (ushort)1) == 1;
+        }
+
+        public void Swap(ref Edge e)
+        {
+            ushort tmpPosition = this.position;
+            ushort tmpHandle = this.handle;
+            this.position = e.position;
+            this.handle = e.handle;
+            e.position = tmpPosition;
+            e.handle = tmpHandle;
+        }
+    }
+
+    public class Handle: BroadphaseProxy
+    {
+        ushort[] minEdges, maxEdges;
+        ushort pad;
+        ushort handleID;
+
+        public ushort[] MinEdges
+        {
+            get { return minEdges; }
+            set { minEdges = value; }
+        }
+
+        public ushort[] MaxEdges
+        {
+            get { return maxEdges; }
+            set { maxEdges = value; }
+        }
+
+        public ushort HandleID
+        {
+            get { return handleID; }
+            set { handleID = value; }
+        }
+
+        public ushort Pad
+        {
+            get { return pad; }
+            set { pad = value; }
+        }
+
+        public ushort NextFree
+        {
+            get { return minEdges[0]; }
+            set { minEdges[0] = value;}
+        }
+
+        public Handle()
+        {
+            minEdges = new ushort[3];
+            maxEdges = new ushort[3];
+        }
+    }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs
index dcfd0d8..eb1fd4b 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs
@@ -1,68 +1,68 @@
-/*

-  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;

-

-namespace XnaDevRu.BulletX

-{

-        /// Dispatcher uses these types

-        /// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave

-        /// to facilitate type checking

-        public enum BroadphaseNativeTypes

-        {

-                // polyhedral convex shapes

-                Box,

-                Triangle,

-                Tetrahedral,

-                ConvexTriangleMesh,

-                ConvexHull,

-                //implicit convex shapes

-                ImplicitConvexShapes,

-                Sphere,

-                MultiSphere,

-                Capsule,

-                Cone,

-                Convex,

-                Cylinder,

-                MinkowskiSum,

-                MinkowskiDifference,

-                //concave shapes

-                ConcaveShapesStart,

-                //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy!

-                TriangleMesh,

-                //used for demo integration FAST/Swift collision library and Bullet

-                FastConcaveMesh,

-                //terrain

-                Terrain,

-                //Used for GIMPACT Trimesh integration

-                Gimpact,

-

-                Empty,

-                StaticPlane,

-                ConcaveShapesEnd,

-

-                Compound,

-

-                MaxBroadphaseCollisionTypes,

-        }

-}

+/*
+  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;
+
+namespace XnaDevRu.BulletX
+{
+        /// Dispatcher uses these types
+        /// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave
+        /// to facilitate type checking
+        public enum BroadphaseNativeTypes
+        {
+                // polyhedral convex shapes
+                Box,
+                Triangle,
+                Tetrahedral,
+                ConvexTriangleMesh,
+                ConvexHull,
+                //implicit convex shapes
+                ImplicitConvexShapes,
+                Sphere,
+                MultiSphere,
+                Capsule,
+                Cone,
+                Convex,
+                Cylinder,
+                MinkowskiSum,
+                MinkowskiDifference,
+                //concave shapes
+                ConcaveShapesStart,
+                //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy!
+                TriangleMesh,
+                //used for demo integration FAST/Swift collision library and Bullet
+                FastConcaveMesh,
+                //terrain
+                Terrain,
+                //Used for GIMPACT Trimesh integration
+                Gimpact,
+
+                Empty,
+                StaticPlane,
+                ConcaveShapesEnd,
+
+                Compound,
+
+                MaxBroadphaseCollisionTypes,
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs
index 2b9e114..eec77ac 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs
@@ -1,113 +1,113 @@
-/*

-  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;

-

-namespace XnaDevRu.BulletX

-{

-        public class BroadphasePair

-        {

-                private BroadphaseProxy _proxyA;

-                private BroadphaseProxy _proxyB;

-

-                private CollisionAlgorithm _algorithm;

-                private object _userInfo;

-

-                public BroadphasePair()

-                {

-                }

-

-                public BroadphasePair(BroadphasePair other)

-                {

-                        _proxyA = other._proxyA;

-                        _proxyB = other._proxyB;

-

-                        _algorithm = other._algorithm;

-                        _userInfo = null;

-                }

-

-                public BroadphasePair(BroadphaseProxy proxyA, BroadphaseProxy proxyB)

-                {

-                        _proxyA = proxyA;

-                        _proxyB = proxyB;

-

-                        _algorithm = null;

-                        _userInfo = null;

-                }

-

-                public BroadphaseProxy ProxyA { get { return _proxyA; } set { _proxyA = value; } }

-                public BroadphaseProxy ProxyB { get { return _proxyB; } set { _proxyB = value; } }

-

-                public CollisionAlgorithm CollisionAlgorithm { get { return _algorithm; } set { _algorithm = value; } }

-                public object UserInfo { get { return _userInfo; } set { _userInfo = value; } }

-

-                public override int GetHashCode()

-                {

-                        return _proxyA.GetHashCode() ^ _proxyB.GetHashCode();

-                }

-

-                public override bool Equals(object obj)

-                {

-                        if (obj is BroadphasePair)

-                                return this == (BroadphasePair)obj;

-                        return false;

-                }

-

-                public static int ComparisonSort(BroadphasePair a, BroadphasePair b)

-                {

-                        int aAId = a.ProxyA != null ? a.ProxyA.ComparisonID : -1;

-                        int aBId = a.ProxyB != null ? a.ProxyB.ComparisonID : -1;

-                        int aCId = a.CollisionAlgorithm != null ? a.CollisionAlgorithm.ComparisonID : -1;

-                        int bAId = b.ProxyA != null ? b.ProxyA.ComparisonID : -1;

-                        int bBId = b.ProxyB != null ? b.ProxyB.ComparisonID : -1;

-                        int bCId = b.CollisionAlgorithm != null ? b.CollisionAlgorithm.ComparisonID : -1;

-                        

-                        if      (aAId > bAId ||

-                                (a.ProxyA == b.ProxyA && aBId > bBId) ||

-                                (a.ProxyA == b.ProxyA && a.ProxyB == b.ProxyB && aCId > bCId))

-                                return -1;

-                        else

-                                return 1;

-                }

-

-                public static bool operator ==(BroadphasePair a, BroadphasePair b)

-                {

-                        if (object.Equals(a, null) && object.Equals(b, null))

-                                return true;

-                        if (object.Equals(a, null) || object.Equals(b, null))

-                                return false;

-

-                        return (a.ProxyA == b.ProxyA) && (a.ProxyB == b.ProxyB);

-                }

-

-                public static bool operator !=(BroadphasePair a, BroadphasePair b)

-                {

-                        if (object.Equals(a, null) && object.Equals(b, null))

-                                return true;

-                        if (object.Equals(a, null) || object.Equals(b, null))

-                                return false;

-

-                        return (a.ProxyA != b.ProxyA) || (a.ProxyB != b.ProxyB);

-                }

-        }

-}

+/*
+  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;
+
+namespace XnaDevRu.BulletX
+{
+        public class BroadphasePair
+        {
+                private BroadphaseProxy _proxyA;
+                private BroadphaseProxy _proxyB;
+
+                private CollisionAlgorithm _algorithm;
+                private object _userInfo;
+
+                public BroadphasePair()
+                {
+                }
+
+                public BroadphasePair(BroadphasePair other)
+                {
+                        _proxyA = other._proxyA;
+                        _proxyB = other._proxyB;
+
+                        _algorithm = other._algorithm;
+                        _userInfo = null;
+                }
+
+                public BroadphasePair(BroadphaseProxy proxyA, BroadphaseProxy proxyB)
+                {
+                        _proxyA = proxyA;
+                        _proxyB = proxyB;
+
+                        _algorithm = null;
+                        _userInfo = null;
+                }
+
+                public BroadphaseProxy ProxyA { get { return _proxyA; } set { _proxyA = value; } }
+                public BroadphaseProxy ProxyB { get { return _proxyB; } set { _proxyB = value; } }
+
+                public CollisionAlgorithm CollisionAlgorithm { get { return _algorithm; } set { _algorithm = value; } }
+                public object UserInfo { get { return _userInfo; } set { _userInfo = value; } }
+
+                public override int GetHashCode()
+                {
+                        return _proxyA.GetHashCode() ^ _proxyB.GetHashCode();
+                }
+
+                public override bool Equals(object obj)
+                {
+                        if (obj is BroadphasePair)
+                                return this == (BroadphasePair)obj;
+                        return false;
+                }
+
+                public static int ComparisonSort(BroadphasePair a, BroadphasePair b)
+                {
+                        int aAId = a.ProxyA != null ? a.ProxyA.ComparisonID : -1;
+                        int aBId = a.ProxyB != null ? a.ProxyB.ComparisonID : -1;
+                        int aCId = a.CollisionAlgorithm != null ? a.CollisionAlgorithm.ComparisonID : -1;
+                        int bAId = b.ProxyA != null ? b.ProxyA.ComparisonID : -1;
+                        int bBId = b.ProxyB != null ? b.ProxyB.ComparisonID : -1;
+                        int bCId = b.CollisionAlgorithm != null ? b.CollisionAlgorithm.ComparisonID : -1;
+                        
+                        if      (aAId > bAId ||
+                                (a.ProxyA == b.ProxyA && aBId > bBId) ||
+                                (a.ProxyA == b.ProxyA && a.ProxyB == b.ProxyB && aCId > bCId))
+                                return -1;
+                        else
+                                return 1;
+                }
+
+                public static bool operator ==(BroadphasePair a, BroadphasePair b)
+                {
+                        if (object.Equals(a, null) && object.Equals(b, null))
+                                return true;
+                        if (object.Equals(a, null) || object.Equals(b, null))
+                                return false;
+
+                        return (a.ProxyA == b.ProxyA) && (a.ProxyB == b.ProxyB);
+                }
+
+                public static bool operator !=(BroadphasePair a, BroadphasePair b)
+                {
+                        if (object.Equals(a, null) && object.Equals(b, null))
+                                return true;
+                        if (object.Equals(a, null) || object.Equals(b, null))
+                                return false;
+
+                        return (a.ProxyA != b.ProxyA) || (a.ProxyB != b.ProxyB);
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs
index aed82d5..0abc613 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs
@@ -1,91 +1,91 @@
-/*

-  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;

-

-namespace XnaDevRu.BulletX

-{

-        public class BroadphaseProxy

-        {

-                //Usually the client CollisionObject or Rigidbody class

-                private object _clientObject;

-                private CollisionFilterGroups _collisionFilterGroup;

-                private CollisionFilterGroups _collisionFilterMask;

-                private readonly int _comparisonID;

-

-                private static int _globalCount = 0;

-

-                public BroadphaseProxy()

-                {

-                        _comparisonID = _globalCount++;

-                }

-

-                public BroadphaseProxy(object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)

-                        : this()

-                {

-                        _clientObject = userData;

-                        _collisionFilterGroup = collisionFilterGroup;

-                        _collisionFilterMask = collisionFilterMask;

-                }

-

-                public object ClientData { get { return _clientObject; } set { _clientObject = value; } }

-                public CollisionFilterGroups CollisionFilterGroup { get { return _collisionFilterGroup; } set { _collisionFilterGroup = value; } }

-                public CollisionFilterGroups CollisionFilterMask { get { return _collisionFilterMask; } set { _collisionFilterMask = value; } }

-                internal int ComparisonID { get { return _comparisonID; } }

-

-                public static bool IsPolyhedral(BroadphaseNativeTypes proxyType)

-                {

-                        return (proxyType < BroadphaseNativeTypes.ImplicitConvexShapes);

-                }

-

-                public static bool IsConvex(BroadphaseNativeTypes proxyType)

-                {

-                        return (proxyType < BroadphaseNativeTypes.ConcaveShapesStart);

-                }

-

-                public static bool IsConcave(BroadphaseNativeTypes proxyType)

-                {

-                        return ((proxyType > BroadphaseNativeTypes.ConcaveShapesStart) &&

-                                (proxyType < BroadphaseNativeTypes.ConcaveShapesEnd));

-                }

-                public static bool IsCompound(BroadphaseNativeTypes proxyType)

-                {

-                        return (proxyType == BroadphaseNativeTypes.Compound);

-                }

-                public static bool IsInfinite(BroadphaseNativeTypes proxyType)

-                {

-                        return (proxyType == BroadphaseNativeTypes.StaticPlane);

-                }

-

-                //optional filtering to cull potential collisions

-                public enum CollisionFilterGroups

-                {

-                        Default = 1,

-                        Static = 2,

-                        Kinematic = 4,

-                        Debris = 8,

-                        Sensor = 16,

-                        All = Default | Static | Kinematic | Debris | Sensor,

-                }

-        }

-}

+/*
+  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;
+
+namespace XnaDevRu.BulletX
+{
+        public class BroadphaseProxy
+        {
+                //Usually the client CollisionObject or Rigidbody class
+                private object _clientObject;
+                private CollisionFilterGroups _collisionFilterGroup;
+                private CollisionFilterGroups _collisionFilterMask;
+                private readonly int _comparisonID;
+
+                private static int _globalCount = 0;
+
+                public BroadphaseProxy()
+                {
+                        _comparisonID = _globalCount++;
+                }
+
+                public BroadphaseProxy(object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)
+                        : this()
+                {
+                        _clientObject = userData;
+                        _collisionFilterGroup = collisionFilterGroup;
+                        _collisionFilterMask = collisionFilterMask;
+                }
+
+                public object ClientData { get { return _clientObject; } set { _clientObject = value; } }
+                public CollisionFilterGroups CollisionFilterGroup { get { return _collisionFilterGroup; } set { _collisionFilterGroup = value; } }
+                public CollisionFilterGroups CollisionFilterMask { get { return _collisionFilterMask; } set { _collisionFilterMask = value; } }
+                internal int ComparisonID { get { return _comparisonID; } }
+
+                public static bool IsPolyhedral(BroadphaseNativeTypes proxyType)
+                {
+                        return (proxyType < BroadphaseNativeTypes.ImplicitConvexShapes);
+                }
+
+                public static bool IsConvex(BroadphaseNativeTypes proxyType)
+                {
+                        return (proxyType < BroadphaseNativeTypes.ConcaveShapesStart);
+                }
+
+                public static bool IsConcave(BroadphaseNativeTypes proxyType)
+                {
+                        return ((proxyType > BroadphaseNativeTypes.ConcaveShapesStart) &&
+                                (proxyType < BroadphaseNativeTypes.ConcaveShapesEnd));
+                }
+                public static bool IsCompound(BroadphaseNativeTypes proxyType)
+                {
+                        return (proxyType == BroadphaseNativeTypes.Compound);
+                }
+                public static bool IsInfinite(BroadphaseNativeTypes proxyType)
+                {
+                        return (proxyType == BroadphaseNativeTypes.StaticPlane);
+                }
+
+                //optional filtering to cull potential collisions
+                public enum CollisionFilterGroups
+                {
+                        Default = 1,
+                        Static = 2,
+                        Kinematic = 4,
+                        Debris = 8,
+                        Sensor = 16,
+                        All = Default | Static | Kinematic | Debris | Sensor,
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs
index 1e59ad8..387340d 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs
@@ -1,51 +1,51 @@
-/*

-  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;

-

-namespace XnaDevRu.BulletX

-{

-        /// <summary>

-        /// CollisionAlgorithm is an collision interface that is compatible with the Broadphase and Dispatcher.

-        /// It is persistent over frames

-        /// </summary>

-        public abstract class CollisionAlgorithm

-        {

-                private IDispatcher _dispatcher;

-                private readonly int _comparisonID = 0;

-

-                private static int _globalCount = 0;

-

-                public CollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo)

-                {

-                        _comparisonID = _globalCount++;

-                        _dispatcher = collisionAlgorithmConstructionInfo.Dispatcher;

-                }

-

-                protected IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } }

-                internal int ComparisonID { get { return _comparisonID; } }

-

-                public abstract void ProcessCollision(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut);

-                public abstract float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut);

-        }

-}

+/*
+  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;
+
+namespace XnaDevRu.BulletX
+{
+        /// <summary>
+        /// CollisionAlgorithm is an collision interface that is compatible with the Broadphase and Dispatcher.
+        /// It is persistent over frames
+        /// </summary>
+        public abstract class CollisionAlgorithm
+        {
+                private IDispatcher _dispatcher;
+                private readonly int _comparisonID = 0;
+
+                private static int _globalCount = 0;
+
+                public CollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo)
+                {
+                        _comparisonID = _globalCount++;
+                        _dispatcher = collisionAlgorithmConstructionInfo.Dispatcher;
+                }
+
+                protected IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } }
+                internal int ComparisonID { get { return _comparisonID; } }
+
+                public abstract void ProcessCollision(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut);
+                public abstract float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut);
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs
index 381752a..de238b9 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.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;

-

-namespace XnaDevRu.BulletX

-{

-        public struct CollisionAlgorithmConstructionInfo

-        {

-                private IDispatcher _dispatcher;

-                private PersistentManifold _manifold;

-

-                public CollisionAlgorithmConstructionInfo(IDispatcher dispatcher)

-                {

-                        _dispatcher = dispatcher;

-                        _manifold = null;

-                }

-

-                public IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } }

-                public PersistentManifold Manifold { get { return _manifold; } set { _manifold = 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;
+
+namespace XnaDevRu.BulletX
+{
+        public struct CollisionAlgorithmConstructionInfo
+        {
+                private IDispatcher _dispatcher;
+                private PersistentManifold _manifold;
+
+                public CollisionAlgorithmConstructionInfo(IDispatcher dispatcher)
+                {
+                        _dispatcher = dispatcher;
+                        _manifold = null;
+                }
+
+                public IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } }
+                public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs
index 8c0367f..cde3221 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs
@@ -1,54 +1,54 @@
-/*

-  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;

-

-namespace XnaDevRu.BulletX

-{

-        public enum DispatchFunction

-        {

-                Discrete = 1,

-                Continuous,

-        }

-

-        public class DispatcherInfo

-        {

-                private float _timeStep;

-                private int _stepCount;

-                private DispatchFunction _dispatchFunc = DispatchFunction.Discrete;

-                private float _timeOfImpact = 1;

-                private bool _useContinuous;

-                private bool _enableSatConvex;

-                private bool _enableSpu;

-                private IDebugDraw _debugDraw;

-

-                public float TimeStep { get { return _timeStep; } set { _timeStep = value; } }

-                public int StepCount { get { return _stepCount; } set { _stepCount = value; } }

-                public DispatchFunction DispatchFunction { get { return _dispatchFunc; } set { _dispatchFunc = value; } }

-                public float TimeOfImpact { get { return _timeOfImpact; } set { _timeOfImpact = value; } }

-                public bool UseContinuous { get { return _useContinuous; } set { _useContinuous = value; } }

-                public bool EnableSatConvex { get { return _enableSatConvex; } set { _enableSatConvex = value; } }

-                public bool enableSpu { get { return _enableSpu; } set { _enableSpu = value; } }

-                public IDebugDraw DebugDraw { get { return _debugDraw; } set { _debugDraw = 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;
+
+namespace XnaDevRu.BulletX
+{
+        public enum DispatchFunction
+        {
+                Discrete = 1,
+                Continuous,
+        }
+
+        public class DispatcherInfo
+        {
+                private float _timeStep;
+                private int _stepCount;
+                private DispatchFunction _dispatchFunc = DispatchFunction.Discrete;
+                private float _timeOfImpact = 1;
+                private bool _useContinuous;
+                private bool _enableSatConvex;
+                private bool _enableSpu;
+                private IDebugDraw _debugDraw;
+
+                public float TimeStep { get { return _timeStep; } set { _timeStep = value; } }
+                public int StepCount { get { return _stepCount; } set { _stepCount = value; } }
+                public DispatchFunction DispatchFunction { get { return _dispatchFunc; } set { _dispatchFunc = value; } }
+                public float TimeOfImpact { get { return _timeOfImpact; } set { _timeOfImpact = value; } }
+                public bool UseContinuous { get { return _useContinuous; } set { _useContinuous = value; } }
+                public bool EnableSatConvex { get { return _enableSatConvex; } set { _enableSatConvex = value; } }
+                public bool enableSpu { get { return _enableSpu; } set { _enableSpu = value; } }
+                public IDebugDraw DebugDraw { get { return _debugDraw; } set { _debugDraw = value; } }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs
index 89618af..fddd8af 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs
@@ -1,36 +1,36 @@
-/*

-  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 interface IBroadphase

-        {

-                BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask);

-                void DestroyProxy(BroadphaseProxy proxy);

-                void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax);

-                void CleanProxyFromPairs(BroadphaseProxy proxy);

-        }

-}

+/*
+  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 interface IBroadphase
+        {
+                BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask);
+                void DestroyProxy(BroadphaseProxy proxy);
+                void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax);
+                void CleanProxyFromPairs(BroadphaseProxy proxy);
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs
index 52831b7..7677179 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.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;

-

-namespace XnaDevRu.BulletX

-{

-        public interface IDispatcher

-        {

-                CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold);

-                CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB);

-                PersistentManifold GetNewManifold(object bodyA, object bodyB);

-                void ReleaseManifold(PersistentManifold manifold);

-                void ClearManifold(PersistentManifold manifold);

-                bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB);

-                bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB);

-                void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo);

-                PersistentManifold GetManifoldByIndex(int index);

-

-                int ManifoldCount { 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;
+
+namespace XnaDevRu.BulletX
+{
+        public interface IDispatcher
+        {
+                CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold);
+                CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB);
+                PersistentManifold GetNewManifold(object bodyA, object bodyB);
+                void ReleaseManifold(PersistentManifold manifold);
+                void ClearManifold(PersistentManifold manifold);
+                bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB);
+                bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB);
+                void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo);
+                PersistentManifold GetManifoldByIndex(int index);
+
+                int ManifoldCount { get; }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs
index 4e128da..2456911 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs
@@ -1,33 +1,33 @@
-/*

-  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;

-

-namespace XnaDevRu.BulletX

-{

-        public interface IOverlapCallback

-        {

-                //return true for deletion of the pair

-                bool ProcessOverlap(ref BroadphasePair pair);

-        }

-}

+/*
+  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;
+
+namespace XnaDevRu.BulletX
+{
+        public interface IOverlapCallback
+        {
+                //return true for deletion of the pair
+                bool ProcessOverlap(ref BroadphasePair pair);
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs
index 165912a..6548560 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs
@@ -1,159 +1,159 @@
-/*

-  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;

-

-namespace XnaDevRu.BulletX

-{

-        public abstract class OverlappingPairCache : IBroadphase

-        {

-                private static int _overlappingPairCount = 0;

-                private List<BroadphasePair> _overlappingPairs = new List<BroadphasePair>();

-                //during the dispatch, check that user doesn't destroy/create proxy

-                private bool _blockedForChanges;

-

-                public List<BroadphasePair> OverlappingPairs { get { return _overlappingPairs; } set { _overlappingPairs = value; } }

-                public bool BlockedForChanges { get { return _blockedForChanges; } set { _blockedForChanges = value; } }

-

-                public static int OverlappingPairCount { get { return _overlappingPairCount; } set { _overlappingPairCount = value; } }

-

-                public void RemoveOverlappingPair(BroadphasePair pair)

-                {

-                        if (!_overlappingPairs.Contains(pair))

-                                return;

-

-                        CleanOverlappingPair(ref pair);

-                        _overlappingPairs.Remove(pair);

-                }

-

-                public void AddOverlappingPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB)

-                {

-                        //don't add overlap with own

-                        bool test = proxyA != proxyB;

-            BulletDebug.Assert(proxyA != proxyB);

-

-                        if (!NeedsBroadphaseCollision(proxyA, proxyB))

-                                return;

-

-                        BroadphasePair pair = new BroadphasePair(proxyA, proxyB);

-                        _overlappingPairs.Add(pair);

-                        _overlappingPairCount++;

-                }

-

-                //this FindPair becomes really slow. Either sort the list to speedup the query, or

-                //use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.

-                //we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)

-                //Also we can use a 2D bitmap, which can be useful for a future GPU implementation

-                public BroadphasePair FindPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB)

-                {

-                        if (!NeedsBroadphaseCollision(proxyA, proxyB))

-                                return null;

-

-                        BroadphasePair pair = new BroadphasePair(proxyA, proxyB);

-                        for (int i = 0; i < _overlappingPairs.Count; i++)

-                        {

-                                if (_overlappingPairs[i] == pair)

-                                {

-                                        return _overlappingPairs[i];

-                                }

-                        }

-

-                        return null;

-                }

-

-                public void CleanProxyFromPairs(BroadphaseProxy proxy)

-                {

-                        for (int i = 0; i < _overlappingPairs.Count; i++)

-                        {

-                                BroadphasePair pair = _overlappingPairs[i];

-                                if (pair.ProxyA == proxy ||

-                                        pair.ProxyB == proxy)

-                                {

-                    CleanOverlappingPair(ref pair);

-                    _overlappingPairs[i] = pair;

-                                }

-                        }

-                }

-

-                public void RemoveOverlappingPairsContainingProxy(BroadphaseProxy proxy)

-                {

-                        for (int i = _overlappingPairs.Count - 1; i >= 0; i--)

-                        {

-                                BroadphasePair pair = _overlappingPairs[i];

-                                if (pair.ProxyA == proxy ||

-                                        pair.ProxyB == proxy)

-                                {

-                                        RemoveOverlappingPair(pair);

-                                        i++;

-                                }

-                        }

-                }

-

-                public bool NeedsBroadphaseCollision(BroadphaseProxy proxy0, BroadphaseProxy proxy1)

-                {

-                        bool collides = (proxy0.CollisionFilterGroup & proxy1.CollisionFilterMask) != 0;

-                        collides = collides && ((proxy1.CollisionFilterGroup & proxy0.CollisionFilterMask) != 0);

-

-                        return collides;

-                }

-

-                public virtual void ProcessAllOverlappingPairs(IOverlapCallback callback)

-                {

-                        List<BroadphasePair> deleting = new List<BroadphasePair>();

-            for (int i = 0; i < _overlappingPairs.Count; i++)

-                        {

-                BroadphasePair p = _overlappingPairs[i];

-                                if (callback.ProcessOverlap(ref p))

-                                {

-                    CleanOverlappingPair(ref p);

-                                        deleting.Add(p);

-                                        _overlappingPairCount--;

-                                }

-                        }

-

-                        for (int i = 0; i < deleting.Count; i++)

-                                _overlappingPairs.Remove(deleting[i]);

-                }

-

-                public void CleanOverlappingPair(ref BroadphasePair pair)

-                {

-            if (pair.CollisionAlgorithm != null)

-            {

-                                if (pair.CollisionAlgorithm is IDisposable)

-                                        (pair.CollisionAlgorithm as IDisposable).Dispose();

-                pair.CollisionAlgorithm = null;

-            }

-                }

-

-                public abstract void RefreshOverlappingPairs();

-

-                #region IBroadphase Members

-                public abstract BroadphaseProxy CreateProxy(MonoXnaCompactMaths.Vector3 min, MonoXnaCompactMaths.Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask);

-

-                public abstract void DestroyProxy(BroadphaseProxy proxy);

-

-                public abstract void SetAabb(BroadphaseProxy proxy, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax);

-

-                #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;
+
+namespace XnaDevRu.BulletX
+{
+        public abstract class OverlappingPairCache : IBroadphase
+        {
+                private static int _overlappingPairCount = 0;
+                private List<BroadphasePair> _overlappingPairs = new List<BroadphasePair>();
+                //during the dispatch, check that user doesn't destroy/create proxy
+                private bool _blockedForChanges;
+
+                public List<BroadphasePair> OverlappingPairs { get { return _overlappingPairs; } set { _overlappingPairs = value; } }
+                public bool BlockedForChanges { get { return _blockedForChanges; } set { _blockedForChanges = value; } }
+
+                public static int OverlappingPairCount { get { return _overlappingPairCount; } set { _overlappingPairCount = value; } }
+
+                public void RemoveOverlappingPair(BroadphasePair pair)
+                {
+                        if (!_overlappingPairs.Contains(pair))
+                                return;
+
+                        CleanOverlappingPair(ref pair);
+                        _overlappingPairs.Remove(pair);
+                }
+
+                public void AddOverlappingPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB)
+                {
+                        //don't add overlap with own
+                        bool test = proxyA != proxyB;
+            BulletDebug.Assert(proxyA != proxyB);
+
+                        if (!NeedsBroadphaseCollision(proxyA, proxyB))
+                                return;
+
+                        BroadphasePair pair = new BroadphasePair(proxyA, proxyB);
+                        _overlappingPairs.Add(pair);
+                        _overlappingPairCount++;
+                }
+
+                //this FindPair becomes really slow. Either sort the list to speedup the query, or
+                //use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.
+                //we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)
+                //Also we can use a 2D bitmap, which can be useful for a future GPU implementation
+                public BroadphasePair FindPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB)
+                {
+                        if (!NeedsBroadphaseCollision(proxyA, proxyB))
+                                return null;
+
+                        BroadphasePair pair = new BroadphasePair(proxyA, proxyB);
+                        for (int i = 0; i < _overlappingPairs.Count; i++)
+                        {
+                                if (_overlappingPairs[i] == pair)
+                                {
+                                        return _overlappingPairs[i];
+                                }
+                        }
+
+                        return null;
+                }
+
+                public void CleanProxyFromPairs(BroadphaseProxy proxy)
+                {
+                        for (int i = 0; i < _overlappingPairs.Count; i++)
+                        {
+                                BroadphasePair pair = _overlappingPairs[i];
+                                if (pair.ProxyA == proxy ||
+                                        pair.ProxyB == proxy)
+                                {
+                    CleanOverlappingPair(ref pair);
+                    _overlappingPairs[i] = pair;
+                                }
+                        }
+                }
+
+                public void RemoveOverlappingPairsContainingProxy(BroadphaseProxy proxy)
+                {
+                        for (int i = _overlappingPairs.Count - 1; i >= 0; i--)
+                        {
+                                BroadphasePair pair = _overlappingPairs[i];
+                                if (pair.ProxyA == proxy ||
+                                        pair.ProxyB == proxy)
+                                {
+                                        RemoveOverlappingPair(pair);
+                                        i++;
+                                }
+                        }
+                }
+
+                public bool NeedsBroadphaseCollision(BroadphaseProxy proxy0, BroadphaseProxy proxy1)
+                {
+                        bool collides = (proxy0.CollisionFilterGroup & proxy1.CollisionFilterMask) != 0;
+                        collides = collides && ((proxy1.CollisionFilterGroup & proxy0.CollisionFilterMask) != 0);
+
+                        return collides;
+                }
+
+                public virtual void ProcessAllOverlappingPairs(IOverlapCallback callback)
+                {
+                        List<BroadphasePair> deleting = new List<BroadphasePair>();
+            for (int i = 0; i < _overlappingPairs.Count; i++)
+                        {
+                BroadphasePair p = _overlappingPairs[i];
+                                if (callback.ProcessOverlap(ref p))
+                                {
+                    CleanOverlappingPair(ref p);
+                                        deleting.Add(p);
+                                        _overlappingPairCount--;
+                                }
+                        }
+
+                        for (int i = 0; i < deleting.Count; i++)
+                                _overlappingPairs.Remove(deleting[i]);
+                }
+
+                public void CleanOverlappingPair(ref BroadphasePair pair)
+                {
+            if (pair.CollisionAlgorithm != null)
+            {
+                                if (pair.CollisionAlgorithm is IDisposable)
+                                        (pair.CollisionAlgorithm as IDisposable).Dispose();
+                pair.CollisionAlgorithm = null;
+            }
+                }
+
+                public abstract void RefreshOverlappingPairs();
+
+                #region IBroadphase Members
+                public abstract BroadphaseProxy CreateProxy(MonoXnaCompactMaths.Vector3 min, MonoXnaCompactMaths.Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask);
+
+                public abstract void DestroyProxy(BroadphaseProxy proxy);
+
+                public abstract void SetAabb(BroadphaseProxy proxy, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax);
+
+                #endregion
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs
index 1dc3f34..f37b3f5 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs
@@ -1,128 +1,128 @@
-/*

-  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 SimpleBroadphase : OverlappingPairCache

-        {

-                private int _maxProxies;

-                private List<SimpleBroadphaseProxy> _proxies = new List<SimpleBroadphaseProxy>();

-

-                public SimpleBroadphase()

-                        : this(16384) { }

-

-                public SimpleBroadphase(int maxProxies)

-                        : base()

-                {

-                        _maxProxies = maxProxies;

-                }

-

-                public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask)

-                {

-                        if (_proxies.Count >= _maxProxies)

-                        {

-                BulletDebug.Assert(false);

-                                return null; //should never happen, but don't let the game crash ;-)

-                        }

-            BulletDebug.Assert(min.X <= max.X && min.Y <= max.Y && min.Z <= max.Z);

-

-                        SimpleBroadphaseProxy proxy = new SimpleBroadphaseProxy(min, max, shapeType, userData, collisionFilterGroup, collisionFilterMask);

-                        _proxies.Add(proxy);

-

-                        return proxy;

-                }

-

-

-                public override void DestroyProxy(BroadphaseProxy proxy)

-                {

-                        RemoveOverlappingPairsContainingProxy(proxy);

-                        _proxies.Remove(proxy as SimpleBroadphaseProxy);

-                }

-

-                public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax)

-                {

-                        SimpleBroadphaseProxy simpleProxy = GetSimpleProxyFromProxy(proxy);

-                        simpleProxy.Minimum = aabbMin;

-                        simpleProxy.Maximum = aabbMax;

-                }

-

-                private SimpleBroadphaseProxy GetSimpleProxyFromProxy(BroadphaseProxy proxy)

-                {

-                        return proxy as SimpleBroadphaseProxy;

-                }

-

-                public override void RefreshOverlappingPairs()

-                {

-                        for (int i = 0; i < _proxies.Count; i++)

-                        {

-                                SimpleBroadphaseProxy proxyA = _proxies[i];

-

-                                for (int j = i + 1; j < _proxies.Count; j++)

-                                {

-                                        SimpleBroadphaseProxy proxyB = _proxies[j];

-

-                                        if (AabbOverlap(proxyA, proxyB))

-                                        {

-                                                if (FindPair(proxyA, proxyB) == null)

-                                                {

-                                                        AddOverlappingPair(proxyA, proxyB);

-                                                }

-                                        }

-                                }

-                        }

-

-                        CheckOverlapCallback check = new CheckOverlapCallback();

-                        ProcessAllOverlappingPairs(check);

-                }

-

-                public static bool AabbOverlap(SimpleBroadphaseProxy proxyA, SimpleBroadphaseProxy proxyB)

-                {

-                        return proxyA.Minimum.X <= proxyB.Maximum.X && proxyB.Minimum.X <= proxyA.Maximum.X &&

-                                   proxyA.Minimum.Y <= proxyB.Maximum.Y && proxyB.Minimum.Y <= proxyA.Maximum.Y &&

-                                   proxyA.Minimum.Z <= proxyB.Maximum.Z && proxyB.Minimum.Z <= proxyA.Maximum.Z;

-                }

-

-                private void Validate()

-                {

-                        for (int i = 0; i < _proxies.Count; i++)

-                        {

-                                for (int j = i + 1; j < _proxies.Count; j++)

-                                {

-                                        if (_proxies[i] == _proxies[j])

-                                                throw new BulletException();

-                                }

-                        }

-                }

-        }

-

-        public class CheckOverlapCallback : IOverlapCallback

-        {

-                public bool ProcessOverlap(ref BroadphasePair pair)

-                {

-                        return (!SimpleBroadphase.AabbOverlap(pair.ProxyA as SimpleBroadphaseProxy, pair.ProxyB as SimpleBroadphaseProxy));

-                }

-        }

-}

+/*
+  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 SimpleBroadphase : OverlappingPairCache
+        {
+                private int _maxProxies;
+                private List<SimpleBroadphaseProxy> _proxies = new List<SimpleBroadphaseProxy>();
+
+                public SimpleBroadphase()
+                        : this(16384) { }
+
+                public SimpleBroadphase(int maxProxies)
+                        : base()
+                {
+                        _maxProxies = maxProxies;
+                }
+
+                public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask)
+                {
+                        if (_proxies.Count >= _maxProxies)
+                        {
+                BulletDebug.Assert(false);
+                                return null; //should never happen, but don't let the game crash ;-)
+                        }
+            BulletDebug.Assert(min.X <= max.X && min.Y <= max.Y && min.Z <= max.Z);
+
+                        SimpleBroadphaseProxy proxy = new SimpleBroadphaseProxy(min, max, shapeType, userData, collisionFilterGroup, collisionFilterMask);
+                        _proxies.Add(proxy);
+
+                        return proxy;
+                }
+
+
+                public override void DestroyProxy(BroadphaseProxy proxy)
+                {
+                        RemoveOverlappingPairsContainingProxy(proxy);
+                        _proxies.Remove(proxy as SimpleBroadphaseProxy);
+                }
+
+                public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax)
+                {
+                        SimpleBroadphaseProxy simpleProxy = GetSimpleProxyFromProxy(proxy);
+                        simpleProxy.Minimum = aabbMin;
+                        simpleProxy.Maximum = aabbMax;
+                }
+
+                private SimpleBroadphaseProxy GetSimpleProxyFromProxy(BroadphaseProxy proxy)
+                {
+                        return proxy as SimpleBroadphaseProxy;
+                }
+
+                public override void RefreshOverlappingPairs()
+                {
+                        for (int i = 0; i < _proxies.Count; i++)
+                        {
+                                SimpleBroadphaseProxy proxyA = _proxies[i];
+
+                                for (int j = i + 1; j < _proxies.Count; j++)
+                                {
+                                        SimpleBroadphaseProxy proxyB = _proxies[j];
+
+                                        if (AabbOverlap(proxyA, proxyB))
+                                        {
+                                                if (FindPair(proxyA, proxyB) == null)
+                                                {
+                                                        AddOverlappingPair(proxyA, proxyB);
+                                                }
+                                        }
+                                }
+                        }
+
+                        CheckOverlapCallback check = new CheckOverlapCallback();
+                        ProcessAllOverlappingPairs(check);
+                }
+
+                public static bool AabbOverlap(SimpleBroadphaseProxy proxyA, SimpleBroadphaseProxy proxyB)
+                {
+                        return proxyA.Minimum.X <= proxyB.Maximum.X && proxyB.Minimum.X <= proxyA.Maximum.X &&
+                                   proxyA.Minimum.Y <= proxyB.Maximum.Y && proxyB.Minimum.Y <= proxyA.Maximum.Y &&
+                                   proxyA.Minimum.Z <= proxyB.Maximum.Z && proxyB.Minimum.Z <= proxyA.Maximum.Z;
+                }
+
+                private void Validate()
+                {
+                        for (int i = 0; i < _proxies.Count; i++)
+                        {
+                                for (int j = i + 1; j < _proxies.Count; j++)
+                                {
+                                        if (_proxies[i] == _proxies[j])
+                                                throw new BulletException();
+                                }
+                        }
+                }
+        }
+
+        public class CheckOverlapCallback : IOverlapCallback
+        {
+                public bool ProcessOverlap(ref BroadphasePair pair)
+                {
+                        return (!SimpleBroadphase.AabbOverlap(pair.ProxyA as SimpleBroadphaseProxy, pair.ProxyB as SimpleBroadphaseProxy));
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs
index f473ef2..cfc2150 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs
@@ -1,46 +1,46 @@
-/*

-  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 SimpleBroadphaseProxy : BroadphaseProxy

-        {

-                private Vector3 _min;

-                private Vector3 _max;

-

-                public SimpleBroadphaseProxy() { }

-

-                public SimpleBroadphaseProxy(Vector3 minPoint, Vector3 maxPoint, BroadphaseNativeTypes shapeType, object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)

-                        : base(userData, collisionFilterGroup, collisionFilterMask)

-                {

-                        _min = minPoint;

-                        _max = maxPoint;

-                }

-

-                public Vector3 Minimum { get { return _min; } set { _min = value; } }

-                public Vector3 Maximum { get { return _max; } set { _max = 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 SimpleBroadphaseProxy : BroadphaseProxy
+        {
+                private Vector3 _min;
+                private Vector3 _max;
+
+                public SimpleBroadphaseProxy() { }
+
+                public SimpleBroadphaseProxy(Vector3 minPoint, Vector3 maxPoint, BroadphaseNativeTypes shapeType, object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)
+                        : base(userData, collisionFilterGroup, collisionFilterMask)
+                {
+                        _min = minPoint;
+                        _max = maxPoint;
+                }
+
+                public Vector3 Minimum { get { return _min; } set { _min = value; } }
+                public Vector3 Maximum { get { return _max; } set { _max = value; } }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs
index ba638d1..6bebf93 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs
@@ -1,59 +1,59 @@
-/*

-  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

-{

-        internal class BridgeTriangleRaycastCallback : TriangleRaycastCallback

-        {

-                CollisionWorld.RayResultCallback _resultCallback;

-                CollisionObject _collisionObject;

-                TriangleMeshShape _triangleMesh;

-

-                public BridgeTriangleRaycastCallback(Vector3 from, Vector3 to,

-                                                                CollisionWorld.RayResultCallback resultCallback, CollisionObject collisionObject, TriangleMeshShape triangleMesh)

-                        : base(from, to)

-                {

-                        _resultCallback = resultCallback;

-                        _collisionObject = collisionObject;

-                        _triangleMesh = triangleMesh;

-                }

-

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

-                {

-                        CollisionWorld.LocalShapeInfo shapeInfo = new CollisionWorld.LocalShapeInfo();

-                        shapeInfo.ShapePart = partId;

-                        shapeInfo.TriangleIndex = triangleIndex;

-

-                        CollisionWorld.LocalRayResult rayResult = new CollisionWorld.LocalRayResult

-                                (_collisionObject,

-                                 shapeInfo,

-                                 hitNormalLocal,

-                                 hitFraction);

-

-                        return _resultCallback.AddSingleResult(rayResult);

-                }

-        }

-}

+/*
+  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
+{
+        internal class BridgeTriangleRaycastCallback : TriangleRaycastCallback
+        {
+                CollisionWorld.RayResultCallback _resultCallback;
+                CollisionObject _collisionObject;
+                TriangleMeshShape _triangleMesh;
+
+                public BridgeTriangleRaycastCallback(Vector3 from, Vector3 to,
+                                                                CollisionWorld.RayResultCallback resultCallback, CollisionObject collisionObject, TriangleMeshShape triangleMesh)
+                        : base(from, to)
+                {
+                        _resultCallback = resultCallback;
+                        _collisionObject = collisionObject;
+                        _triangleMesh = triangleMesh;
+                }
+
+                public override float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex)
+                {
+                        CollisionWorld.LocalShapeInfo shapeInfo = new CollisionWorld.LocalShapeInfo();
+                        shapeInfo.ShapePart = partId;
+                        shapeInfo.TriangleIndex = triangleIndex;
+
+                        CollisionWorld.LocalRayResult rayResult = new CollisionWorld.LocalRayResult
+                                (_collisionObject,
+                                 shapeInfo,
+                                 hitNormalLocal,
+                                 hitFraction);
+
+                        return _resultCallback.AddSingleResult(rayResult);
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs
index 4e3903b..3bd5601 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs
@@ -1,40 +1,40 @@
-/*

-  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 CollisionAlgorithmCreateFunction

-        {

-                private bool _swapped;

-

-                public bool IsSwapped { get { return _swapped; } set { _swapped = value; } }

-

-                public virtual CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1)

-                {

-                        return null;

-                }

-        }

-}

+/*
+  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 CollisionAlgorithmCreateFunction
+        {
+                private bool _swapped;
+
+                public bool IsSwapped { get { return _swapped; } set { _swapped = value; } }
+
+                public virtual CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1)
+                {
+                        return null;
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs
index 75d35b0..336defd 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs
@@ -1,280 +1,280 @@
-/*

-  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 void NearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo);

-

-        public class CollisionDispatcher : IDispatcher

-        {

-                private List<PersistentManifold> _manifolds = new List<PersistentManifold>();

-

-                //private bool _useIslands;

-                private NearCallback _nearCallback;

-

-                //private ManifoldResult _defaultManifoldResult;

-

-                private CollisionAlgorithmCreateFunction[,] _doubleDispatch = new CollisionAlgorithmCreateFunction[(int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes, (int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes];

-

-                //default CreationFunctions, filling the m_doubleDispatch table

-                private CollisionAlgorithmCreateFunction _convexConvexCreateFunc;

-                private CollisionAlgorithmCreateFunction _convexConcaveCreateFunc;

-                private CollisionAlgorithmCreateFunction _swappedConvexConcaveCreateFunc;

-                private CollisionAlgorithmCreateFunction _compoundCreateFunc;

-                private CollisionAlgorithmCreateFunction _swappedCompoundCreateFunc;

-                private CollisionAlgorithmCreateFunction _emptyCreateFunc;

-

-                private int _count;

-                private static int _manifoldCount = 0;

-

-                public CollisionDispatcher()

-                {

-                        NearCallback = DefaultNearCallback;

-                        //_useIslands = true;

-                        //default CreationFunctions, filling the m_doubleDispatch table

-                        _convexConvexCreateFunc = new ConvexConvexAlgorithm.CreateFunc();

-                        _convexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.CreateFunc();

-                        _swappedConvexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.SwappedCreateFunc();

-                        _compoundCreateFunc = new CompoundCollisionAlgorithm.CreateFunc();

-                        _swappedCompoundCreateFunc = new CompoundCollisionAlgorithm.SwappedCreateFunc();

-                        _emptyCreateFunc = new EmptyAlgorithm.CreateFunc();

-

-                        for (BroadphaseNativeTypes i = BroadphaseNativeTypes.Box; i < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; i++)

-                        {

-                                for (BroadphaseNativeTypes j = BroadphaseNativeTypes.Box; j < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; j++)

-                                {

-                                        _doubleDispatch[(int)i, (int)j] = FindCreateFunction(i, j);

-                                        if (_doubleDispatch[(int)i, (int)j] == null)

-                                                throw new BulletException();

-                                }

-                        }

-                }

-

-                public int Count { get { return _count; } set { _count = value; } }

-                public int ManifoldCount { get { return _manifolds.Count; } }

-                public List<PersistentManifold> Manifolds { get { return _manifolds; } }

-

-                public static int GlobalManifoldCount { get { return _manifoldCount; } set { _manifoldCount = value; } }

-

-                public PersistentManifold GetManifoldByIndex(int index)

-                {

-                        return _manifolds[index];

-                }

-

-                //registerCollisionCreateFunc allows registration of custom/alternative collision create functions

-                public void RegisterCollisionCreateFunc(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB, CollisionAlgorithmCreateFunction createFunc)

-                {

-                        _doubleDispatch[(int)proxyTypeA, (int)proxyTypeB] = createFunc;

-                }

-

-                public virtual PersistentManifold GetNewManifold(object bodyA, object bodyB)

-                {

-                        _manifoldCount++;

-

-                        CollisionObject body0 = bodyA as CollisionObject;

-                        CollisionObject body1 = bodyB as CollisionObject;

-

-                        PersistentManifold manifold = new PersistentManifold(body0, body1);

-                        _manifolds.Add(manifold);

-

-                        return manifold;

-                }

-

-                public virtual void ReleaseManifold(PersistentManifold manifold)

-                {

-                        _manifoldCount--;

-

-                        ClearManifold(manifold);

-                        _manifolds.Remove(manifold);

-                }

-

-

-                public virtual void ClearManifold(PersistentManifold manifold)

-                {

-                        manifold.ClearManifold();

-                }

-

-                public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB)

-                {

-                        return FindAlgorithm(bodyA, bodyB, null);

-                }

-

-                public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold)

-                {

-                        CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo();

-                        collisionAlgorithmConstructionInfo.Dispatcher = this;

-                        collisionAlgorithmConstructionInfo.Manifold = sharedManifold;

-                        CollisionAlgorithm collisionAlgorithm = _doubleDispatch[(int)bodyA.CollisionShape.ShapeType, (int)bodyB.CollisionShape.ShapeType].CreateCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB);

-                        return collisionAlgorithm;

-                }

-

-                /*public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1)

-                {

-                        return internalFindAlgorithm(body0, body1, null);

-                }

-

-                public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold)

-                {

-                        m_count++;

-

-                        CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo();

-                        ci.m_dispatcher = this;

-

-                        if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConvex())

-                        {

-                                return new ConvexConvexAlgorithm(sharedManifold, ci, body0, body1);

-                        }

-

-                        if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConcave())

-                        {

-                                return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, false);

-                        }

-

-                        if (body1.getCollisionShape().isConvex() && body0.getCollisionShape().isConcave())

-                        {

-                                return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, true);

-                        }

-

-                        if (body0.getCollisionShape().isCompound())

-                        {

-                                return new CompoundCollisionAlgorithm(ci, body0, body1, false);

-                        }

-                        else

-                        {

-                                if (body1.getCollisionShape().isCompound())

-                                {

-                                        return new CompoundCollisionAlgorithm(ci, body0, body1, true);

-                                }

-                        }

-

-                        //failed to find an algorithm

-                        return new EmptyAlgorithm(ci);

-                }*/

-

-                public virtual bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB)

-                {

-                        if (bodyA == null || bodyB == null)

-                                throw new BulletException();

-

-                        bool needsCollision = true;

-

-                        //broadphase filtering already deals with this

-                        /*if ((body0.isStaticObject() || body0.isKinematicObject()) &&

-                                (body1.isStaticObject() || body1.isKinematicObject()))

-                        {

-                                printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n");

-                        }*/

-

-                        if ((!bodyA.IsActive) && (!bodyB.IsActive))

-                                needsCollision = false;

-

-                        return needsCollision;

-                }

-

-                public virtual bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB)

-                {

-                        //here you can do filtering

-                        bool hasResponse = bodyA.HasContactResponse && bodyB.HasContactResponse;

-                        hasResponse = hasResponse && (!bodyA.IsStaticOrKinematicObject || !bodyB.IsStaticOrKinematicObject);

-                        return hasResponse;

-                }

-

-                public virtual void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo)

-                {

-                        CollisionPairCallback collisionCallback = new CollisionPairCallback(dispatchInfo, this);

-                        pairCache.ProcessAllOverlappingPairs(collisionCallback);

-                }

-

-                private CollisionAlgorithmCreateFunction FindCreateFunction(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB)

-                {

-                        if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConvex(proxyTypeB))

-                        {

-                                return _convexConvexCreateFunc;

-                        }

-

-                        if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConcave(proxyTypeB))

-                        {

-                                return _convexConcaveCreateFunc;

-                        }

-

-                        if (BroadphaseProxy.IsConvex(proxyTypeB) && BroadphaseProxy.IsConcave(proxyTypeA))

-                        {

-                                return _swappedConvexConcaveCreateFunc;

-                        }

-

-                        if (BroadphaseProxy.IsCompound(proxyTypeA))

-                        {

-                                return _compoundCreateFunc;

-                        }

-                        else

-                        {

-                                if (BroadphaseProxy.IsCompound(proxyTypeB))

-                                {

-                                        return _swappedCompoundCreateFunc;

-                                }

-                        }

-

-                        //failed to find an algorithm

-                        return _emptyCreateFunc;

-                }

-

-                public NearCallback NearCallback { get { return _nearCallback; } set { _nearCallback = value; } }

-

-                //by default, Bullet will use this near callback

-                public static void DefaultNearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo)

-                {

-                        CollisionObject collisionObjectA = collisionPair.ProxyA.ClientData as CollisionObject;

-                        CollisionObject collisionObjectB = collisionPair.ProxyB.ClientData as CollisionObject;

-

-                        if (dispatcher.NeedsCollision(collisionObjectA, collisionObjectB))

-                        {

-                                //dispatcher will keep algorithms persistent in the collision pair

-                                if (collisionPair.CollisionAlgorithm == null)

-                                {

-                                        collisionPair.CollisionAlgorithm = dispatcher.FindAlgorithm(collisionObjectA, collisionObjectB);

-                                }

-

-                                if (collisionPair.CollisionAlgorithm != null)

-                                {

-                                        ManifoldResult contactPointResult = new ManifoldResult(collisionObjectA, collisionObjectB);

-

-                                        if (dispatchInfo.DispatchFunction == DispatchFunction.Discrete)

-                                        {

-                                                //discrete collision detection query

-                                                collisionPair.CollisionAlgorithm.ProcessCollision(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult);

-                                        }

-                                        else

-                                        {

-                                                //continuous collision detection query, time of impact (toi)

-                                                float timeOfImpact = collisionPair.CollisionAlgorithm.CalculateTimeOfImpact(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult);

-                                                if (dispatchInfo.TimeOfImpact > timeOfImpact)

-                                                        dispatchInfo.TimeOfImpact = timeOfImpact;

-                                        }

-                                }

-                        }

-                }

-        }

-}

+/*
+  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 void NearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo);
+
+        public class CollisionDispatcher : IDispatcher
+        {
+                private List<PersistentManifold> _manifolds = new List<PersistentManifold>();
+
+                //private bool _useIslands;
+                private NearCallback _nearCallback;
+
+                //private ManifoldResult _defaultManifoldResult;
+
+                private CollisionAlgorithmCreateFunction[,] _doubleDispatch = new CollisionAlgorithmCreateFunction[(int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes, (int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes];
+
+                //default CreationFunctions, filling the m_doubleDispatch table
+                private CollisionAlgorithmCreateFunction _convexConvexCreateFunc;
+                private CollisionAlgorithmCreateFunction _convexConcaveCreateFunc;
+                private CollisionAlgorithmCreateFunction _swappedConvexConcaveCreateFunc;
+                private CollisionAlgorithmCreateFunction _compoundCreateFunc;
+                private CollisionAlgorithmCreateFunction _swappedCompoundCreateFunc;
+                private CollisionAlgorithmCreateFunction _emptyCreateFunc;
+
+                private int _count;
+                private static int _manifoldCount = 0;
+
+                public CollisionDispatcher()
+                {
+                        NearCallback = DefaultNearCallback;
+                        //_useIslands = true;
+                        //default CreationFunctions, filling the m_doubleDispatch table
+                        _convexConvexCreateFunc = new ConvexConvexAlgorithm.CreateFunc();
+                        _convexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.CreateFunc();
+                        _swappedConvexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.SwappedCreateFunc();
+                        _compoundCreateFunc = new CompoundCollisionAlgorithm.CreateFunc();
+                        _swappedCompoundCreateFunc = new CompoundCollisionAlgorithm.SwappedCreateFunc();
+                        _emptyCreateFunc = new EmptyAlgorithm.CreateFunc();
+
+                        for (BroadphaseNativeTypes i = BroadphaseNativeTypes.Box; i < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; i++)
+                        {
+                                for (BroadphaseNativeTypes j = BroadphaseNativeTypes.Box; j < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; j++)
+                                {
+                                        _doubleDispatch[(int)i, (int)j] = FindCreateFunction(i, j);
+                                        if (_doubleDispatch[(int)i, (int)j] == null)
+                                                throw new BulletException();
+                                }
+                        }
+                }
+
+                public int Count { get { return _count; } set { _count = value; } }
+                public int ManifoldCount { get { return _manifolds.Count; } }
+                public List<PersistentManifold> Manifolds { get { return _manifolds; } }
+
+                public static int GlobalManifoldCount { get { return _manifoldCount; } set { _manifoldCount = value; } }
+
+                public PersistentManifold GetManifoldByIndex(int index)
+                {
+                        return _manifolds[index];
+                }
+
+                //registerCollisionCreateFunc allows registration of custom/alternative collision create functions
+                public void RegisterCollisionCreateFunc(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB, CollisionAlgorithmCreateFunction createFunc)
+                {
+                        _doubleDispatch[(int)proxyTypeA, (int)proxyTypeB] = createFunc;
+                }
+
+                public virtual PersistentManifold GetNewManifold(object bodyA, object bodyB)
+                {
+                        _manifoldCount++;
+
+                        CollisionObject body0 = bodyA as CollisionObject;
+                        CollisionObject body1 = bodyB as CollisionObject;
+
+                        PersistentManifold manifold = new PersistentManifold(body0, body1);
+                        _manifolds.Add(manifold);
+
+                        return manifold;
+                }
+
+                public virtual void ReleaseManifold(PersistentManifold manifold)
+                {
+                        _manifoldCount--;
+
+                        ClearManifold(manifold);
+                        _manifolds.Remove(manifold);
+                }
+
+
+                public virtual void ClearManifold(PersistentManifold manifold)
+                {
+                        manifold.ClearManifold();
+                }
+
+                public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB)
+                {
+                        return FindAlgorithm(bodyA, bodyB, null);
+                }
+
+                public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold)
+                {
+                        CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo();
+                        collisionAlgorithmConstructionInfo.Dispatcher = this;
+                        collisionAlgorithmConstructionInfo.Manifold = sharedManifold;
+                        CollisionAlgorithm collisionAlgorithm = _doubleDispatch[(int)bodyA.CollisionShape.ShapeType, (int)bodyB.CollisionShape.ShapeType].CreateCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB);
+                        return collisionAlgorithm;
+                }
+
+                /*public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1)
+                {
+                        return internalFindAlgorithm(body0, body1, null);
+                }
+
+                public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold)
+                {
+                        m_count++;
+
+                        CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo();
+                        ci.m_dispatcher = this;
+
+                        if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConvex())
+                        {
+                                return new ConvexConvexAlgorithm(sharedManifold, ci, body0, body1);
+                        }
+
+                        if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConcave())
+                        {
+                                return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, false);
+                        }
+
+                        if (body1.getCollisionShape().isConvex() && body0.getCollisionShape().isConcave())
+                        {
+                                return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, true);
+                        }
+
+                        if (body0.getCollisionShape().isCompound())
+                        {
+                                return new CompoundCollisionAlgorithm(ci, body0, body1, false);
+                        }
+                        else
+                        {
+                                if (body1.getCollisionShape().isCompound())
+                                {
+                                        return new CompoundCollisionAlgorithm(ci, body0, body1, true);
+                                }
+                        }
+
+                        //failed to find an algorithm
+                        return new EmptyAlgorithm(ci);
+                }*/
+
+                public virtual bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB)
+                {
+                        if (bodyA == null || bodyB == null)
+                                throw new BulletException();
+
+                        bool needsCollision = true;
+
+                        //broadphase filtering already deals with this
+                        /*if ((body0.isStaticObject() || body0.isKinematicObject()) &&
+                                (body1.isStaticObject() || body1.isKinematicObject()))
+                        {
+                                printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n");
+                        }*/
+
+                        if ((!bodyA.IsActive) && (!bodyB.IsActive))
+                                needsCollision = false;
+
+                        return needsCollision;
+                }
+
+                public virtual bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB)
+                {
+                        //here you can do filtering
+                        bool hasResponse = bodyA.HasContactResponse && bodyB.HasContactResponse;
+                        hasResponse = hasResponse && (!bodyA.IsStaticOrKinematicObject || !bodyB.IsStaticOrKinematicObject);
+                        return hasResponse;
+                }
+
+                public virtual void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo)
+                {
+                        CollisionPairCallback collisionCallback = new CollisionPairCallback(dispatchInfo, this);
+                        pairCache.ProcessAllOverlappingPairs(collisionCallback);
+                }
+
+                private CollisionAlgorithmCreateFunction FindCreateFunction(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB)
+                {
+                        if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConvex(proxyTypeB))
+                        {
+                                return _convexConvexCreateFunc;
+                        }
+
+                        if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConcave(proxyTypeB))
+                        {
+                                return _convexConcaveCreateFunc;
+                        }
+
+                        if (BroadphaseProxy.IsConvex(proxyTypeB) && BroadphaseProxy.IsConcave(proxyTypeA))
+                        {
+                                return _swappedConvexConcaveCreateFunc;
+                        }
+
+                        if (BroadphaseProxy.IsCompound(proxyTypeA))
+                        {
+                                return _compoundCreateFunc;
+                        }
+                        else
+                        {
+                                if (BroadphaseProxy.IsCompound(proxyTypeB))
+                                {
+                                        return _swappedCompoundCreateFunc;
+                                }
+                        }
+
+                        //failed to find an algorithm
+                        return _emptyCreateFunc;
+                }
+
+                public NearCallback NearCallback { get { return _nearCallback; } set { _nearCallback = value; } }
+
+                //by default, Bullet will use this near callback
+                public static void DefaultNearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo)
+                {
+                        CollisionObject collisionObjectA = collisionPair.ProxyA.ClientData as CollisionObject;
+                        CollisionObject collisionObjectB = collisionPair.ProxyB.ClientData as CollisionObject;
+
+                        if (dispatcher.NeedsCollision(collisionObjectA, collisionObjectB))
+                        {
+                                //dispatcher will keep algorithms persistent in the collision pair
+                                if (collisionPair.CollisionAlgorithm == null)
+                                {
+                                        collisionPair.CollisionAlgorithm = dispatcher.FindAlgorithm(collisionObjectA, collisionObjectB);
+                                }
+
+                                if (collisionPair.CollisionAlgorithm != null)
+                                {
+                                        ManifoldResult contactPointResult = new ManifoldResult(collisionObjectA, collisionObjectB);
+
+                                        if (dispatchInfo.DispatchFunction == DispatchFunction.Discrete)
+                                        {
+                                                //discrete collision detection query
+                                                collisionPair.CollisionAlgorithm.ProcessCollision(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult);
+                                        }
+                                        else
+                                        {
+                                                //continuous collision detection query, time of impact (toi)
+                                                float timeOfImpact = collisionPair.CollisionAlgorithm.CalculateTimeOfImpact(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult);
+                                                if (dispatchInfo.TimeOfImpact > timeOfImpact)
+                                                        dispatchInfo.TimeOfImpact = timeOfImpact;
+                                        }
+                                }
+                        }
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs
index 4e9cf58..3eae564 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs
@@ -1,163 +1,163 @@
-/*

-  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 enum ActivationState

-        {

-                Nothing = 0,

-                Active,

-                IslandSleeping,

-                WantsDeactivation,

-                DisableDeactivation,

-                DisableSimulation,

-        }

-

-        public enum CollisionOptions

-        {

-                StaticObject = 1,

-                KinematicObject = 2,

-                NoContactResponse = 4,

-                CustomMaterialCallback = 8,//this allows per-triangle material (friction/restitution)

-        }

-

-        /// <summary>

-        /// btCollisionObject can be used to manage collision detection objects. 

-        /// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy.

-        /// They can be added to the btCollisionWorld.

-        /// </summary>

-        public class CollisionObject

-        {

-                protected Matrix _worldTransform;

-                private BroadphaseProxy _broadphase;

-                private CollisionShape _collisionShape;

-

-                //m_interpolationWorldTransform is used for CCD and interpolation

-                //it can be either previous or future (predicted) transform

-                private Matrix _interpolationWorldTransform;

-

-                private CollisionOptions _collisionFlags;

-

-                private int _islandTag;

-                private ActivationState _activationState;

-                private float _deactivationTime;

-

-                private float _friction;

-                private float _restitution;

-

-                //users can point to their objects, m_userPointer is not used by Bullet

-                private object _userData;

-

-                //m_internalOwner one is used by optional Bullet high level interface

-                private object _internalOwner;

-

-                //time of impact calculation

-                private float _hitFraction;

-

-                //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::

-                private float _ccdSweptSphereRadius;

-

-                // Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionTreshold

-                private float _ccdSquareMotionThreshold;

-                //those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) 

-                //without destroying the continuous interpolated motion (which uses this interpolation velocities)

-                private Vector3 _interpolationLinearVelocity;

-                private Vector3 _interpolationAngularVelocity;

-

-                private int _companionID;

-

-                public CollisionObject()

-                {

-                        _activationState = ActivationState.Active;

-                        _hitFraction = 1;

-                }

-

-                public bool IsStaticObject { get { return (_collisionFlags & CollisionOptions.StaticObject) != 0; } }

-                public bool IsKinematicObject { get { return (_collisionFlags & CollisionOptions.KinematicObject) != 0; } }

-                public bool IsStaticOrKinematicObject { get { return (_collisionFlags & (CollisionOptions.KinematicObject | CollisionOptions.StaticObject)) != 0; } }

-

-                public bool HasContactResponse { get { return (_collisionFlags & CollisionOptions.NoContactResponse) == 0; } }

-                public bool MergesSimulationIslands

-                {

-                        get

-                        {

-                                //static objects, kinematic and object without contact response don't merge islands

-                                return (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject | CollisionOptions.NoContactResponse)) == 0;

-                        }

-                }

-

-                public ActivationState ActivationState

-                {

-                        get { return _activationState; }

-                        set

-                        {

-                                if ((_activationState != ActivationState.DisableDeactivation) && (_activationState != ActivationState.DisableSimulation))

-                                        _activationState = value;

-                        }

-                }

-

-                public bool IsActive { get { return ((ActivationState != ActivationState.IslandSleeping) && (ActivationState != ActivationState.DisableSimulation)); } }

-                public float Restitution { get { return _restitution; } set { _restitution = value; } }

-                public float Friction { get { return _friction; } set { _friction = value; } }

-                public CollisionShape CollisionShape { get { return _collisionShape; } set { _collisionShape = value; } }

-                public float DeactivationTime { get { return _deactivationTime; } set { _deactivationTime = value; } }

-                public object Owner { get { return _internalOwner; } protected set { _internalOwner = value; } }

-                public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } }

-                public BroadphaseProxy Broadphase { get { return _broadphase; } set { _broadphase = value; } }

-                public Matrix InterpolationWorldTransform { get { return _interpolationWorldTransform; } set { _interpolationWorldTransform = value; } }

-                public Vector3 InterpolationLinearVelocity { get { return _interpolationLinearVelocity; } protected set { _interpolationLinearVelocity = value; } }

-                public Vector3 InterpolationAngularVelocity { get { return _interpolationAngularVelocity; } protected set { _interpolationAngularVelocity = value; } }

-                public int IslandTag { get { return _islandTag; } set { _islandTag = value; } }

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

-                public CollisionOptions CollisionFlags { get { return _collisionFlags; } set { _collisionFlags = value; } }

-                //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm

-                public float CcdSweptSphereRadius { get { return _ccdSweptSphereRadius; } set { _ccdSweptSphereRadius = value; } }

-                // Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold

-                public float CcdSquareMotionThreshold { get { return _ccdSquareMotionThreshold; } set { _ccdSquareMotionThreshold = value; } }

-                //users can point to their objects, userPointer is not used by Bullet

-                public object UserData { get { return _userData; } set { _userData = value; } }

-                public int CompanionID { get { return _companionID; } set { _companionID = value; } }

-

-                public void ForceActivationState(ActivationState newState)

-                {

-                        _activationState = newState;

-                }

-

-                public void Activate()

-                {

-                        Activate(false);

-                }

-

-                public void Activate(bool forceActivation)

-                {

-                        if (forceActivation || (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject)) == 0)

-                        {

-                                ActivationState = ActivationState.Active;

-                                _deactivationTime = 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 enum ActivationState
+        {
+                Nothing = 0,
+                Active,
+                IslandSleeping,
+                WantsDeactivation,
+                DisableDeactivation,
+                DisableSimulation,
+        }
+
+        public enum CollisionOptions
+        {
+                StaticObject = 1,
+                KinematicObject = 2,
+                NoContactResponse = 4,
+                CustomMaterialCallback = 8,//this allows per-triangle material (friction/restitution)
+        }
+
+        /// <summary>
+        /// btCollisionObject can be used to manage collision detection objects. 
+        /// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy.
+        /// They can be added to the btCollisionWorld.
+        /// </summary>
+        public class CollisionObject
+        {
+                protected Matrix _worldTransform;
+                private BroadphaseProxy _broadphase;
+                private CollisionShape _collisionShape;
+
+                //m_interpolationWorldTransform is used for CCD and interpolation
+                //it can be either previous or future (predicted) transform
+                private Matrix _interpolationWorldTransform;
+
+                private CollisionOptions _collisionFlags;
+
+                private int _islandTag;
+                private ActivationState _activationState;
+                private float _deactivationTime;
+
+                private float _friction;
+                private float _restitution;
+
+                //users can point to their objects, m_userPointer is not used by Bullet
+                private object _userData;
+
+                //m_internalOwner one is used by optional Bullet high level interface
+                private object _internalOwner;
+
+                //time of impact calculation
+                private float _hitFraction;
+
+                //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
+                private float _ccdSweptSphereRadius;
+
+                // Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionTreshold
+                private float _ccdSquareMotionThreshold;
+                //those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities) 
+                //without destroying the continuous interpolated motion (which uses this interpolation velocities)
+                private Vector3 _interpolationLinearVelocity;
+                private Vector3 _interpolationAngularVelocity;
+
+                private int _companionID;
+
+                public CollisionObject()
+                {
+                        _activationState = ActivationState.Active;
+                        _hitFraction = 1;
+                }
+
+                public bool IsStaticObject { get { return (_collisionFlags & CollisionOptions.StaticObject) != 0; } }
+                public bool IsKinematicObject { get { return (_collisionFlags & CollisionOptions.KinematicObject) != 0; } }
+                public bool IsStaticOrKinematicObject { get { return (_collisionFlags & (CollisionOptions.KinematicObject | CollisionOptions.StaticObject)) != 0; } }
+
+                public bool HasContactResponse { get { return (_collisionFlags & CollisionOptions.NoContactResponse) == 0; } }
+                public bool MergesSimulationIslands
+                {
+                        get
+                        {
+                                //static objects, kinematic and object without contact response don't merge islands
+                                return (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject | CollisionOptions.NoContactResponse)) == 0;
+                        }
+                }
+
+                public ActivationState ActivationState
+                {
+                        get { return _activationState; }
+                        set
+                        {
+                                if ((_activationState != ActivationState.DisableDeactivation) && (_activationState != ActivationState.DisableSimulation))
+                                        _activationState = value;
+                        }
+                }
+
+                public bool IsActive { get { return ((ActivationState != ActivationState.IslandSleeping) && (ActivationState != ActivationState.DisableSimulation)); } }
+                public float Restitution { get { return _restitution; } set { _restitution = value; } }
+                public float Friction { get { return _friction; } set { _friction = value; } }
+                public CollisionShape CollisionShape { get { return _collisionShape; } set { _collisionShape = value; } }
+                public float DeactivationTime { get { return _deactivationTime; } set { _deactivationTime = value; } }
+                public object Owner { get { return _internalOwner; } protected set { _internalOwner = value; } }
+                public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } }
+                public BroadphaseProxy Broadphase { get { return _broadphase; } set { _broadphase = value; } }
+                public Matrix InterpolationWorldTransform { get { return _interpolationWorldTransform; } set { _interpolationWorldTransform = value; } }
+                public Vector3 InterpolationLinearVelocity { get { return _interpolationLinearVelocity; } protected set { _interpolationLinearVelocity = value; } }
+                public Vector3 InterpolationAngularVelocity { get { return _interpolationAngularVelocity; } protected set { _interpolationAngularVelocity = value; } }
+                public int IslandTag { get { return _islandTag; } set { _islandTag = value; } }
+                public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } }
+                public CollisionOptions CollisionFlags { get { return _collisionFlags; } set { _collisionFlags = value; } }
+                //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm
+                public float CcdSweptSphereRadius { get { return _ccdSweptSphereRadius; } set { _ccdSweptSphereRadius = value; } }
+                // Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold
+                public float CcdSquareMotionThreshold { get { return _ccdSquareMotionThreshold; } set { _ccdSquareMotionThreshold = value; } }
+                //users can point to their objects, userPointer is not used by Bullet
+                public object UserData { get { return _userData; } set { _userData = value; } }
+                public int CompanionID { get { return _companionID; } set { _companionID = value; } }
+
+                public void ForceActivationState(ActivationState newState)
+                {
+                        _activationState = newState;
+                }
+
+                public void Activate()
+                {
+                        Activate(false);
+                }
+
+                public void Activate(bool forceActivation)
+                {
+                        if (forceActivation || (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject)) == 0)
+                        {
+                                ActivationState = ActivationState.Active;
+                                _deactivationTime = 0;
+                        }
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs
index 7a85731..6ba1c87 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.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

-{

-        public class CollisionPairCallback : IOverlapCallback

-        {

-                private DispatcherInfo _dispatchInfo;

-                private CollisionDispatcher _dispatcher;

-

-                public CollisionPairCallback(DispatcherInfo dispatchInfo, CollisionDispatcher dispatcher)

-                {

-                        _dispatchInfo = dispatchInfo;

-                        _dispatcher = dispatcher;

-                }

-

-                #region IOverlapCallback Members

-                public bool ProcessOverlap(ref BroadphasePair pair)

-                {

-                        _dispatcher.NearCallback(ref pair, _dispatcher, _dispatchInfo);

-                        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
+{
+        public class CollisionPairCallback : IOverlapCallback
+        {
+                private DispatcherInfo _dispatchInfo;
+                private CollisionDispatcher _dispatcher;
+
+                public CollisionPairCallback(DispatcherInfo dispatchInfo, CollisionDispatcher dispatcher)
+                {
+                        _dispatchInfo = dispatchInfo;
+                        _dispatcher = dispatcher;
+                }
+
+                #region IOverlapCallback Members
+                public bool ProcessOverlap(ref BroadphasePair pair)
+                {
+                        _dispatcher.NearCallback(ref pair, _dispatcher, _dispatchInfo);
+                        return false;
+                }
+                #endregion
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs
index 968e219..cb520bb 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs
@@ -1,358 +1,358 @@
-/*

-  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 CollisionWorld

-        {

-                private List<CollisionObject> _collisionObjects = new List<CollisionObject>();

-                private IDispatcher _dispatcher;

-                private OverlappingPairCache _broadphasePairCache;

-                private bool _ownsDispatcher;

-                private bool _ownsBroadphasePairCache;

-                private DispatcherInfo _dispatchInfo = new DispatcherInfo();

-

-                /// <summary>

-                /// this constructor doesn't own the dispatcher and paircache/broadphase

-                /// </summary>

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

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

-                public CollisionWorld(IDispatcher dispatcher, OverlappingPairCache pairCache)

-                {

-                        _dispatcher = dispatcher;

-                        _broadphasePairCache = pairCache;

-                        _ownsDispatcher = false;

-                        _ownsBroadphasePairCache = false;

-                }

-

-                public DispatcherInfo DispatchInfo { get { return _dispatchInfo; } protected set { _dispatchInfo = value; } }

-                public List<CollisionObject> CollisionObjects { get { return _collisionObjects; } protected set { _collisionObjects = value; } }

-                public IBroadphase Broadphase { get { return _broadphasePairCache; } }

-                public OverlappingPairCache BroadphasePairCache { get { return _broadphasePairCache; } protected set { _broadphasePairCache = value; } }

-                public IDispatcher Dispatcher { get { return _dispatcher; } protected set { _dispatcher = value; } }

-                public int CollisionObjectsCount { get { return _collisionObjects.Count; } }

-                protected bool OwnsDispatcher { get { return _ownsDispatcher; } set { _ownsDispatcher = value; } }

-                protected bool OwnsBroadphasePairCache { get { return _ownsBroadphasePairCache; } set { _ownsBroadphasePairCache = value; } }

-

-                // rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback

-                // This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback.

-                public void RayTest(Vector3 rayFromWorld, Vector3 rayToWorld, RayResultCallback resultCallback)

-                {

-                        Matrix rayFromTrans, rayToTrans;

-

-                        rayFromTrans = Matrix.Identity;

-                        rayFromTrans.Translation = rayFromWorld;

-

-                        rayToTrans = Matrix.Identity;

-                        rayToTrans.Translation = rayToWorld;

-

-                        // brute force go over all objects. Once there is a broadphase, use that, or

-                        // add a raycast against aabb first.

-

-                        foreach (CollisionObject collisionObject in _collisionObjects)

-                        {

-                                //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();

-                                Vector3 collisionObjectAabbMin, collisionObjectAabbMax;

-                                collisionObject.CollisionShape.GetAabb(collisionObject.WorldTransform, out collisionObjectAabbMin, out collisionObjectAabbMax);

-

-                                float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing

-                                Vector3 hitNormal = new Vector3();

-                                

-                                //if (MathHelper.TestAabbAgainstAabb2(rayAabbMin, rayAabbMax, collisionObjectAabbMin, collisionObjectAabbMax))

-                                if (MathHelper.RayAabb(rayFromWorld, rayToWorld, collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal))

-                                {

-                                        RayTestSingle(rayFromTrans, rayToTrans,

-                                                collisionObject, collisionObject.CollisionShape,

-                                                         collisionObject.WorldTransform, resultCallback);

-

-                                }

-                        }

-                }

-

-                // rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.

-                // In a future implementation, we consider moving the ray test as a virtual method in CollisionShape.

-                // This allows more customization.

-                public static void RayTestSingle(Matrix rayFromTrans, Matrix rayToTrans,

-                                                  CollisionObject collisionObject,

-                                                  CollisionShape collisionShape,

-                                                  Matrix colObjWorldTransform,

-                                                  RayResultCallback resultCallback)

-                {

-                        SphereShape pointShape=new SphereShape(0.0f);

-

-                        if (collisionShape.IsConvex)

-                        {

-                                CastResult castResult = new CastResult();

-                                castResult.Fraction = 1f;//??

-

-                                ConvexShape convexShape = collisionShape as ConvexShape;

-                                VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();

-                                SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, convexShape, simplexSolver);

-                                //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);

-                                //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);

-

-                                if (convexCaster.CalcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult))

-                                {

-                                        //add hit

-                                        if (castResult.Normal.LengthSquared() > 0.0001f)

-                                        {

-                                                castResult.Normal.Normalize();

-                                                if (castResult.Fraction < resultCallback.ClosestHitFraction)

-                                                {

-

-                                                        CollisionWorld.LocalRayResult localRayResult = new LocalRayResult

-                                                                (

-                                                                        collisionObject,

-                                                                        new LocalShapeInfo(),

-                                                                        castResult.Normal,

-                                                                        castResult.Fraction

-                                                                );

-

-                                                        resultCallback.AddSingleResult(localRayResult);

-                                                }

-                                        }

-                                }

-                                else

-                                {

-                                        if (collisionShape.IsConcave)

-                                        {

-

-                                                TriangleMeshShape triangleMesh = collisionShape as TriangleMeshShape;

-

-                                                Matrix worldTocollisionObject = MathHelper.InvertMatrix(colObjWorldTransform);

-

-                                                Vector3 rayFromLocal = Vector3.TransformNormal(rayFromTrans.Translation, worldTocollisionObject);

-                                                Vector3 rayToLocal = Vector3.TransformNormal(rayToTrans.Translation, worldTocollisionObject);

-

-                                                BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh);

-                                                rcb.HitFraction = resultCallback.ClosestHitFraction;

-

-                                                Vector3 rayAabbMinLocal = rayFromLocal;

-                                                MathHelper.SetMin(ref rayAabbMinLocal, rayToLocal);

-                                                Vector3 rayAabbMaxLocal = rayFromLocal;

-                                                MathHelper.SetMax(ref rayAabbMaxLocal, rayToLocal);

-

-                                                triangleMesh.ProcessAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal);

-                                        }

-                                        else

-                                        {

-                                                //todo: use AABB tree or other BVH acceleration structure!

-                                                if (collisionShape.IsCompound)

-                                                {

-                                                        CompoundShape compoundShape = collisionShape as CompoundShape;

-                                                        for (int i = 0; i < compoundShape.ChildShapeCount; i++)

-                                                        {

-                                                                Matrix childTrans = compoundShape.GetChildTransform(i);

-                                                                CollisionShape childCollisionShape = compoundShape.GetChildShape(i);

-                                                                Matrix childWorldTrans = colObjWorldTransform * childTrans;

-                                                                RayTestSingle(rayFromTrans, rayToTrans,

-                                                                        collisionObject,

-                                                                        childCollisionShape,

-                                                                        childWorldTrans,

-                                                                        resultCallback);

-                                                        }

-                                                }

-                                        }

-                                }

-                        }

-                }

-

-                public void AddCollisionObject(CollisionObject collisionObject, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask)

-                {

-                        //check that the object isn't already added

-                        if (!_collisionObjects.Contains(collisionObject))

-                        {

-                                _collisionObjects.Add(collisionObject);

-

-                                //calculate new AABB

-                                Matrix trans = collisionObject.WorldTransform;

-

-                                Vector3 minAabb;

-                                Vector3 maxAabb;

-                                collisionObject.CollisionShape.GetAabb(trans, out minAabb, out maxAabb);

-

-                                BroadphaseNativeTypes type = collisionObject.CollisionShape.ShapeType;

-                                collisionObject.Broadphase = Broadphase.CreateProxy(

-                                        minAabb,

-                                        maxAabb,

-                                        type,

-                                        collisionObject,

-                                        collisionFilterGroup,

-                                        collisionFilterMask

-                                        );

-                        }

-                }

-

-                public void AddCollisionObject(CollisionObject collisionObject)

-                {

-                        AddCollisionObject(collisionObject, BroadphaseProxy.CollisionFilterGroups.Default, BroadphaseProxy.CollisionFilterGroups.Default);

-                }

-

-                public void RemoveCollisionObject(CollisionObject collisionObject)

-                {

-                        BroadphaseProxy bp = collisionObject.Broadphase;

-                        if (bp != null)

-                        {

-                                //

-                                // only clear the cached algorithms

-                                //

-                                Broadphase.CleanProxyFromPairs(bp);

-                                Broadphase.DestroyProxy(bp);

-                                collisionObject.Broadphase = null;

-                        }

-

-                        _collisionObjects.Remove(collisionObject);

-                }

-

-                public virtual void PerformDiscreteCollisionDetection()

-                {

-                        DispatcherInfo dispatchInfo = DispatchInfo;

-                        //update aabb (of all moved objects)

-

-                        Vector3 aabbMin, aabbMax;

-                        for (int i = 0; i < _collisionObjects.Count; i++)

-                        {

-                                _collisionObjects[i].CollisionShape.GetAabb(_collisionObjects[i].WorldTransform, out aabbMin, out aabbMax);

-                                _broadphasePairCache.SetAabb(_collisionObjects[i].Broadphase, aabbMin, aabbMax);

-                        }

-

-                        _broadphasePairCache.RefreshOverlappingPairs();

-

-                        IDispatcher dispatcher = Dispatcher;

-                        if (dispatcher != null)

-                                dispatcher.DispatchAllCollisionPairs(_broadphasePairCache, dispatchInfo);

-                }

-

-                public void Dispose(bool disposing)

-                {

-                        if (disposing)

-                        {

-                                //clean up remaining objects

-                                foreach (CollisionObject collisionObject in _collisionObjects)

-                                {

-                                        BroadphaseProxy bp = collisionObject.Broadphase;

-                                        if (bp != null)

-                                        {

-                                                //

-                                                // only clear the cached algorithms

-                                                //

-                                                Broadphase.CleanProxyFromPairs(bp);

-                                                Broadphase.DestroyProxy(bp);

-                                        }

-                                }

-                        }

-                }

-

-                /// <summary>

-                /// LocalShapeInfo gives extra information for complex shapes

-                /// Currently, only TriangleMeshShape is available, so it just contains triangleIndex and subpart

-                /// </summary>

-                public struct LocalShapeInfo

-                {

-                        private int _shapePart;

-                        private int _triangleIndex;

-

-                        public int ShapePart { get { return _shapePart; } set { _shapePart = value; } }

-                        public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } }

-                }

-

-                public struct LocalRayResult

-                {

-                        private CollisionObject _collisionObject;

-                        private LocalShapeInfo _localShapeInfo;

-                        private Vector3 _hitNormalLocal;

-                        private float _hitFraction;

-

-                        public LocalRayResult(CollisionObject collisionObject,

-                                LocalShapeInfo localShapeInfo,

-                                Vector3 hitNormalLocal,

-                                float hitFraction)

-                        {

-                                _collisionObject = collisionObject;

-                                _localShapeInfo = localShapeInfo;

-                                _hitNormalLocal = hitNormalLocal;

-                                _hitFraction = hitFraction;

-                        }

-

-                        public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } }

-                        public LocalShapeInfo LocalShapeInfo { get { return _localShapeInfo; } set { _localShapeInfo = value; } }

-                        public Vector3 HitNormalLocal { get { return _hitNormalLocal; } set { _hitNormalLocal = value; } }

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

-                }

-

-                /// <summary>

-                /// RayResultCallback is used to report new raycast results

-                /// </summary>

-                public abstract class RayResultCallback

-                {

-                        private float _closestHitFraction;

-

-                        public RayResultCallback()

-                        {

-                                _closestHitFraction = 1;

-                        }

-

-                        public float ClosestHitFraction { get { return _closestHitFraction; } set { _closestHitFraction = value; } }

-                        public bool HasHit { get { return _closestHitFraction < 1; } }

-

-                        public abstract float AddSingleResult(LocalRayResult rayResult);

-                }

-

-                public class ClosestRayResultCallback : RayResultCallback

-                {

-                        private Vector3 _rayFromWorld;//used to calculate hitPointWorld from hitFraction

-                        private Vector3 _rayToWorld;

-

-                        private Vector3 _hitNormalWorld;

-                        private Vector3 _hitPointWorld;

-                        private CollisionObject _collisionObject;

-

-                        public ClosestRayResultCallback(Vector3 rayFromWorld, Vector3 rayToWorld)

-                        {

-                                _rayFromWorld = rayFromWorld;

-                                _rayToWorld = rayToWorld;

-                                _collisionObject = null;

-                        }

-

-                        public Vector3 RayFromWorld { get { return _rayFromWorld; } set { _rayFromWorld = value; } }

-                        public Vector3 RayToWorld { get { return _rayToWorld; } set { _rayToWorld = value; } }

-                        public Vector3 HitNormalWorld { get { return _hitNormalWorld; } set { _hitNormalWorld = value; } }

-                        public Vector3 HitPointWorld { get { return _hitPointWorld; } set { _hitPointWorld = value; } }

-                        public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } }

-

-                        public override float AddSingleResult(LocalRayResult rayResult)

-                        {

-                                //caller already does the filter on the m_closestHitFraction

-                                //assert(rayResult.m_hitFraction <= m_closestHitFraction);

-                                ClosestHitFraction = rayResult.HitFraction;

-                                _collisionObject = rayResult.CollisionObject;

-                                _hitNormalWorld = Vector3.TransformNormal(rayResult.HitNormalLocal, _collisionObject.WorldTransform);

-                                MathHelper.SetInterpolate3(_rayFromWorld, _rayToWorld, rayResult.HitFraction, ref _hitPointWorld);

-                                return rayResult.HitFraction;

-                        }

-                }

-        }

-}

+/*
+  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 CollisionWorld
+        {
+                private List<CollisionObject> _collisionObjects = new List<CollisionObject>();
+                private IDispatcher _dispatcher;
+                private OverlappingPairCache _broadphasePairCache;
+                private bool _ownsDispatcher;
+                private bool _ownsBroadphasePairCache;
+                private DispatcherInfo _dispatchInfo = new DispatcherInfo();
+
+                /// <summary>
+                /// this constructor doesn't own the dispatcher and paircache/broadphase
+                /// </summary>
+                /// <param name="dispatcher"></param>
+                /// <param name="pairCache"></param>
+                public CollisionWorld(IDispatcher dispatcher, OverlappingPairCache pairCache)
+                {
+                        _dispatcher = dispatcher;
+                        _broadphasePairCache = pairCache;
+                        _ownsDispatcher = false;
+                        _ownsBroadphasePairCache = false;
+                }
+
+                public DispatcherInfo DispatchInfo { get { return _dispatchInfo; } protected set { _dispatchInfo = value; } }
+                public List<CollisionObject> CollisionObjects { get { return _collisionObjects; } protected set { _collisionObjects = value; } }
+                public IBroadphase Broadphase { get { return _broadphasePairCache; } }
+                public OverlappingPairCache BroadphasePairCache { get { return _broadphasePairCache; } protected set { _broadphasePairCache = value; } }
+                public IDispatcher Dispatcher { get { return _dispatcher; } protected set { _dispatcher = value; } }
+                public int CollisionObjectsCount { get { return _collisionObjects.Count; } }
+                protected bool OwnsDispatcher { get { return _ownsDispatcher; } set { _ownsDispatcher = value; } }
+                protected bool OwnsBroadphasePairCache { get { return _ownsBroadphasePairCache; } set { _ownsBroadphasePairCache = value; } }
+
+                // rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback
+                // This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback.
+                public void RayTest(Vector3 rayFromWorld, Vector3 rayToWorld, RayResultCallback resultCallback)
+                {
+                        Matrix rayFromTrans, rayToTrans;
+
+                        rayFromTrans = Matrix.Identity;
+                        rayFromTrans.Translation = rayFromWorld;
+
+                        rayToTrans = Matrix.Identity;
+                        rayToTrans.Translation = rayToWorld;
+
+                        // brute force go over all objects. Once there is a broadphase, use that, or
+                        // add a raycast against aabb first.
+
+                        foreach (CollisionObject collisionObject in _collisionObjects)
+                        {
+                                //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
+                                Vector3 collisionObjectAabbMin, collisionObjectAabbMax;
+                                collisionObject.CollisionShape.GetAabb(collisionObject.WorldTransform, out collisionObjectAabbMin, out collisionObjectAabbMax);
+
+                                float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing
+                                Vector3 hitNormal = new Vector3();
+                                
+                                //if (MathHelper.TestAabbAgainstAabb2(rayAabbMin, rayAabbMax, collisionObjectAabbMin, collisionObjectAabbMax))
+                                if (MathHelper.RayAabb(rayFromWorld, rayToWorld, collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal))
+                                {
+                                        RayTestSingle(rayFromTrans, rayToTrans,
+                                                collisionObject, collisionObject.CollisionShape,
+                                                         collisionObject.WorldTransform, resultCallback);
+
+                                }
+                        }
+                }
+
+                // rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
+                // In a future implementation, we consider moving the ray test as a virtual method in CollisionShape.
+                // This allows more customization.
+                public static void RayTestSingle(Matrix rayFromTrans, Matrix rayToTrans,
+                                                  CollisionObject collisionObject,
+                                                  CollisionShape collisionShape,
+                                                  Matrix colObjWorldTransform,
+                                                  RayResultCallback resultCallback)
+                {
+                        SphereShape pointShape=new SphereShape(0.0f);
+
+                        if (collisionShape.IsConvex)
+                        {
+                                CastResult castResult = new CastResult();
+                                castResult.Fraction = 1f;//??
+
+                                ConvexShape convexShape = collisionShape as ConvexShape;
+                                VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
+                                SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, convexShape, simplexSolver);
+                                //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
+                                //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
+
+                                if (convexCaster.CalcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult))
+                                {
+                                        //add hit
+                                        if (castResult.Normal.LengthSquared() > 0.0001f)
+                                        {
+                                                castResult.Normal.Normalize();
+                                                if (castResult.Fraction < resultCallback.ClosestHitFraction)
+                                                {
+
+                                                        CollisionWorld.LocalRayResult localRayResult = new LocalRayResult
+                                                                (
+                                                                        collisionObject,
+                                                                        new LocalShapeInfo(),
+                                                                        castResult.Normal,
+                                                                        castResult.Fraction
+                                                                );
+
+                                                        resultCallback.AddSingleResult(localRayResult);
+                                                }
+                                        }
+                                }
+                                else
+                                {
+                                        if (collisionShape.IsConcave)
+                                        {
+
+                                                TriangleMeshShape triangleMesh = collisionShape as TriangleMeshShape;
+
+                                                Matrix worldTocollisionObject = MathHelper.InvertMatrix(colObjWorldTransform);
+
+                                                Vector3 rayFromLocal = Vector3.TransformNormal(rayFromTrans.Translation, worldTocollisionObject);
+                                                Vector3 rayToLocal = Vector3.TransformNormal(rayToTrans.Translation, worldTocollisionObject);
+
+                                                BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh);
+                                                rcb.HitFraction = resultCallback.ClosestHitFraction;
+
+                                                Vector3 rayAabbMinLocal = rayFromLocal;
+                                                MathHelper.SetMin(ref rayAabbMinLocal, rayToLocal);
+                                                Vector3 rayAabbMaxLocal = rayFromLocal;
+                                                MathHelper.SetMax(ref rayAabbMaxLocal, rayToLocal);
+
+                                                triangleMesh.ProcessAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal);
+                                        }
+                                        else
+                                        {
+                                                //todo: use AABB tree or other BVH acceleration structure!
+                                                if (collisionShape.IsCompound)
+                                                {
+                                                        CompoundShape compoundShape = collisionShape as CompoundShape;
+                                                        for (int i = 0; i < compoundShape.ChildShapeCount; i++)
+                                                        {
+                                                                Matrix childTrans = compoundShape.GetChildTransform(i);
+                                                                CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
+                                                                Matrix childWorldTrans = colObjWorldTransform * childTrans;
+                                                                RayTestSingle(rayFromTrans, rayToTrans,
+                                                                        collisionObject,
+                                                                        childCollisionShape,
+                                                                        childWorldTrans,
+                                                                        resultCallback);
+                                                        }
+                                                }
+                                        }
+                                }
+                        }
+                }
+
+                public void AddCollisionObject(CollisionObject collisionObject, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask)
+                {
+                        //check that the object isn't already added
+                        if (!_collisionObjects.Contains(collisionObject))
+                        {
+                                _collisionObjects.Add(collisionObject);
+
+                                //calculate new AABB
+                                Matrix trans = collisionObject.WorldTransform;
+
+                                Vector3 minAabb;
+                                Vector3 maxAabb;
+                                collisionObject.CollisionShape.GetAabb(trans, out minAabb, out maxAabb);
+
+                                BroadphaseNativeTypes type = collisionObject.CollisionShape.ShapeType;
+                                collisionObject.Broadphase = Broadphase.CreateProxy(
+                                        minAabb,
+                                        maxAabb,
+                                        type,
+                                        collisionObject,
+                                        collisionFilterGroup,
+                                        collisionFilterMask
+                                        );
+                        }
+                }
+
+                public void AddCollisionObject(CollisionObject collisionObject)
+                {
+                        AddCollisionObject(collisionObject, BroadphaseProxy.CollisionFilterGroups.Default, BroadphaseProxy.CollisionFilterGroups.Default);
+                }
+
+                public void RemoveCollisionObject(CollisionObject collisionObject)
+                {
+                        BroadphaseProxy bp = collisionObject.Broadphase;
+                        if (bp != null)
+                        {
+                                //
+                                // only clear the cached algorithms
+                                //
+                                Broadphase.CleanProxyFromPairs(bp);
+                                Broadphase.DestroyProxy(bp);
+                                collisionObject.Broadphase = null;
+                        }
+
+                        _collisionObjects.Remove(collisionObject);
+                }
+
+                public virtual void PerformDiscreteCollisionDetection()
+                {
+                        DispatcherInfo dispatchInfo = DispatchInfo;
+                        //update aabb (of all moved objects)
+
+                        Vector3 aabbMin, aabbMax;
+                        for (int i = 0; i < _collisionObjects.Count; i++)
+                        {
+                                _collisionObjects[i].CollisionShape.GetAabb(_collisionObjects[i].WorldTransform, out aabbMin, out aabbMax);
+                                _broadphasePairCache.SetAabb(_collisionObjects[i].Broadphase, aabbMin, aabbMax);
+                        }
+
+                        _broadphasePairCache.RefreshOverlappingPairs();
+
+                        IDispatcher dispatcher = Dispatcher;
+                        if (dispatcher != null)
+                                dispatcher.DispatchAllCollisionPairs(_broadphasePairCache, dispatchInfo);
+                }
+
+                public void Dispose(bool disposing)
+                {
+                        if (disposing)
+                        {
+                                //clean up remaining objects
+                                foreach (CollisionObject collisionObject in _collisionObjects)
+                                {
+                                        BroadphaseProxy bp = collisionObject.Broadphase;
+                                        if (bp != null)
+                                        {
+                                                //
+                                                // only clear the cached algorithms
+                                                //
+                                                Broadphase.CleanProxyFromPairs(bp);
+                                                Broadphase.DestroyProxy(bp);
+                                        }
+                                }
+                        }
+                }
+
+                /// <summary>
+                /// LocalShapeInfo gives extra information for complex shapes
+                /// Currently, only TriangleMeshShape is available, so it just contains triangleIndex and subpart
+                /// </summary>
+                public struct LocalShapeInfo
+                {
+                        private int _shapePart;
+                        private int _triangleIndex;
+
+                        public int ShapePart { get { return _shapePart; } set { _shapePart = value; } }
+                        public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } }
+                }
+
+                public struct LocalRayResult
+                {
+                        private CollisionObject _collisionObject;
+                        private LocalShapeInfo _localShapeInfo;
+                        private Vector3 _hitNormalLocal;
+                        private float _hitFraction;
+
+                        public LocalRayResult(CollisionObject collisionObject,
+                                LocalShapeInfo localShapeInfo,
+                                Vector3 hitNormalLocal,
+                                float hitFraction)
+                        {
+                                _collisionObject = collisionObject;
+                                _localShapeInfo = localShapeInfo;
+                                _hitNormalLocal = hitNormalLocal;
+                                _hitFraction = hitFraction;
+                        }
+
+                        public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } }
+                        public LocalShapeInfo LocalShapeInfo { get { return _localShapeInfo; } set { _localShapeInfo = value; } }
+                        public Vector3 HitNormalLocal { get { return _hitNormalLocal; } set { _hitNormalLocal = value; } }
+                        public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } }
+                }
+
+                /// <summary>
+                /// RayResultCallback is used to report new raycast results
+                /// </summary>
+                public abstract class RayResultCallback
+                {
+                        private float _closestHitFraction;
+
+                        public RayResultCallback()
+                        {
+                                _closestHitFraction = 1;
+                        }
+
+                        public float ClosestHitFraction { get { return _closestHitFraction; } set { _closestHitFraction = value; } }
+                        public bool HasHit { get { return _closestHitFraction < 1; } }
+
+                        public abstract float AddSingleResult(LocalRayResult rayResult);
+                }
+
+                public class ClosestRayResultCallback : RayResultCallback
+                {
+                        private Vector3 _rayFromWorld;//used to calculate hitPointWorld from hitFraction
+                        private Vector3 _rayToWorld;
+
+                        private Vector3 _hitNormalWorld;
+                        private Vector3 _hitPointWorld;
+                        private CollisionObject _collisionObject;
+
+                        public ClosestRayResultCallback(Vector3 rayFromWorld, Vector3 rayToWorld)
+                        {
+                                _rayFromWorld = rayFromWorld;
+                                _rayToWorld = rayToWorld;
+                                _collisionObject = null;
+                        }
+
+                        public Vector3 RayFromWorld { get { return _rayFromWorld; } set { _rayFromWorld = value; } }
+                        public Vector3 RayToWorld { get { return _rayToWorld; } set { _rayToWorld = value; } }
+                        public Vector3 HitNormalWorld { get { return _hitNormalWorld; } set { _hitNormalWorld = value; } }
+                        public Vector3 HitPointWorld { get { return _hitPointWorld; } set { _hitPointWorld = value; } }
+                        public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } }
+
+                        public override float AddSingleResult(LocalRayResult rayResult)
+                        {
+                                //caller already does the filter on the m_closestHitFraction
+                                //assert(rayResult.m_hitFraction <= m_closestHitFraction);
+                                ClosestHitFraction = rayResult.HitFraction;
+                                _collisionObject = rayResult.CollisionObject;
+                                _hitNormalWorld = Vector3.TransformNormal(rayResult.HitNormalLocal, _collisionObject.WorldTransform);
+                                MathHelper.SetInterpolate3(_rayFromWorld, _rayToWorld, rayResult.HitFraction, ref _hitPointWorld);
+                                return rayResult.HitFraction;
+                        }
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs
index 3fec7d0..6f2278a 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs
@@ -1,157 +1,157 @@
-/*

-  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 System.Diagnostics;

-using MonoXnaCompactMaths;

-

-namespace XnaDevRu.BulletX

-{

-        public class CompoundCollisionAlgorithm : CollisionAlgorithm

-        {

-                private List<CollisionAlgorithm> _childCollisionAlgorithms;

-                private bool _isSwapped;

-

-                public CompoundCollisionAlgorithm(

-                        CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo,

-                        CollisionObject bodyA,

-                        CollisionObject bodyB, bool isSwapped)

-                        : base(collisionAlgorithmConstructionInfo)

-                {

-                        //Begin

-                        _isSwapped = isSwapped;

-

-                        CollisionObject collisionObject = isSwapped ? bodyB : bodyA;

-                        CollisionObject otherObject = isSwapped ? bodyA : bodyB;

-

-                        BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);

-

-                        CompoundShape compoundShape = collisionObject.CollisionShape as CompoundShape;

-                        int childrenNumber = compoundShape.ChildShapeCount;

-                        int index = 0;

-

-                        _childCollisionAlgorithms = new List<CollisionAlgorithm>(childrenNumber);

-

-                        for (index = 0; index < childrenNumber; index++)

-                        {

-                                CollisionShape childShape = compoundShape.GetChildShape(index);

-                                CollisionShape orgShape = collisionObject.CollisionShape;

-

-                                collisionObject.CollisionShape = childShape;

-                                _childCollisionAlgorithms[index] = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(collisionObject, otherObject);

-                                collisionObject.CollisionShape = orgShape;

-                        }

-                }

-

-                public override void ProcessCollision(

-                        CollisionObject bodyA,

-                        CollisionObject bodyB,

-                        DispatcherInfo dispatchInfo, ManifoldResult resultOut)

-                {

-                        //Begin

-

-                        CollisionObject collisionObject = _isSwapped ? bodyB : bodyB;

-                        CollisionObject otherObject = _isSwapped ? bodyA : bodyB;

-

-                        //Debug.Assert(collisionObject.getCollisionShape().isCompound());

-                        BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);

-

-                        CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape;

-

-                        int childrenNumber = _childCollisionAlgorithms.Count;

-

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

-                        {

-                                CompoundShape childShape = compoundShape.GetChildShape(i) as CompoundShape;

-

-                                Matrix orgTransform = collisionObject.WorldTransform;

-                                CollisionShape orgShape = collisionObject.CollisionShape;

-

-                                Matrix childTransform = compoundShape.GetChildTransform(i);

-                                Matrix newChildWorld = orgTransform * childTransform;

-

-                                collisionObject.WorldTransform = newChildWorld;

-                                collisionObject.CollisionShape = childShape;

-                                _childCollisionAlgorithms[i].ProcessCollision(collisionObject, otherObject, dispatchInfo, resultOut);

-

-                                collisionObject.CollisionShape = orgShape;

-                                collisionObject.WorldTransform = orgTransform;

-                        }

-                }

-

-                public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)

-                {

-                        CollisionObject collisionObject = _isSwapped ? bodyB : bodyA;

-                        CollisionObject otherObject = _isSwapped ? bodyA : bodyB;

-

-                        BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);

-

-                        CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape;

-

-                        float hitFraction = 1.0f;

-

-                        for (int i = 0; i < _childCollisionAlgorithms.Count; i++)

-                        {

-                                CollisionShape childShape = compoundShape.GetChildShape(i);

-

-                                Matrix orgTransform = collisionObject.WorldTransform;

-                                CollisionShape orgShape = collisionObject.CollisionShape;

-

-                                Matrix childTransform = compoundShape.GetChildTransform(i);

-                                Matrix newChildWorld = orgTransform * childTransform;

-                                collisionObject.WorldTransform = newChildWorld;

-

-                                collisionObject.CollisionShape = childShape;

-                                float frac = _childCollisionAlgorithms[i].CalculateTimeOfImpact(

-                                        collisionObject, otherObject, dispatchInfo, resultOut

-                                );

-

-                                if (frac < hitFraction)

-                                {

-                                        hitFraction = frac;

-                                }

-

-                                collisionObject.CollisionShape = orgShape;

-                                collisionObject.WorldTransform = orgTransform;

-                        }

-

-                        return hitFraction;

-                }

-

-                public class CreateFunc : CollisionAlgorithmCreateFunction

-                {

-                        public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)

-                        {

-                                return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false);

-                        }

-                };

-

-                public class SwappedCreateFunc : CollisionAlgorithmCreateFunction

-                {

-                        public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)

-                        {

-                                return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, 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 System.Diagnostics;
+using MonoXnaCompactMaths;
+
+namespace XnaDevRu.BulletX
+{
+        public class CompoundCollisionAlgorithm : CollisionAlgorithm
+        {
+                private List<CollisionAlgorithm> _childCollisionAlgorithms;
+                private bool _isSwapped;
+
+                public CompoundCollisionAlgorithm(
+                        CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo,
+                        CollisionObject bodyA,
+                        CollisionObject bodyB, bool isSwapped)
+                        : base(collisionAlgorithmConstructionInfo)
+                {
+                        //Begin
+                        _isSwapped = isSwapped;
+
+                        CollisionObject collisionObject = isSwapped ? bodyB : bodyA;
+                        CollisionObject otherObject = isSwapped ? bodyA : bodyB;
+
+                        BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);
+
+                        CompoundShape compoundShape = collisionObject.CollisionShape as CompoundShape;
+                        int childrenNumber = compoundShape.ChildShapeCount;
+                        int index = 0;
+
+                        _childCollisionAlgorithms = new List<CollisionAlgorithm>(childrenNumber);
+
+                        for (index = 0; index < childrenNumber; index++)
+                        {
+                                CollisionShape childShape = compoundShape.GetChildShape(index);
+                                CollisionShape orgShape = collisionObject.CollisionShape;
+
+                                collisionObject.CollisionShape = childShape;
+                                _childCollisionAlgorithms[index] = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(collisionObject, otherObject);
+                                collisionObject.CollisionShape = orgShape;
+                        }
+                }
+
+                public override void ProcessCollision(
+                        CollisionObject bodyA,
+                        CollisionObject bodyB,
+                        DispatcherInfo dispatchInfo, ManifoldResult resultOut)
+                {
+                        //Begin
+
+                        CollisionObject collisionObject = _isSwapped ? bodyB : bodyB;
+                        CollisionObject otherObject = _isSwapped ? bodyA : bodyB;
+
+                        //Debug.Assert(collisionObject.getCollisionShape().isCompound());
+                        BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);
+
+                        CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape;
+
+                        int childrenNumber = _childCollisionAlgorithms.Count;
+
+                        for (int i = 0; i < childrenNumber; i++)
+                        {
+                                CompoundShape childShape = compoundShape.GetChildShape(i) as CompoundShape;
+
+                                Matrix orgTransform = collisionObject.WorldTransform;
+                                CollisionShape orgShape = collisionObject.CollisionShape;
+
+                                Matrix childTransform = compoundShape.GetChildTransform(i);
+                                Matrix newChildWorld = orgTransform * childTransform;
+
+                                collisionObject.WorldTransform = newChildWorld;
+                                collisionObject.CollisionShape = childShape;
+                                _childCollisionAlgorithms[i].ProcessCollision(collisionObject, otherObject, dispatchInfo, resultOut);
+
+                                collisionObject.CollisionShape = orgShape;
+                                collisionObject.WorldTransform = orgTransform;
+                        }
+                }
+
+                public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
+                {
+                        CollisionObject collisionObject = _isSwapped ? bodyB : bodyA;
+                        CollisionObject otherObject = _isSwapped ? bodyA : bodyB;
+
+                        BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);
+
+                        CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape;
+
+                        float hitFraction = 1.0f;
+
+                        for (int i = 0; i < _childCollisionAlgorithms.Count; i++)
+                        {
+                                CollisionShape childShape = compoundShape.GetChildShape(i);
+
+                                Matrix orgTransform = collisionObject.WorldTransform;
+                                CollisionShape orgShape = collisionObject.CollisionShape;
+
+                                Matrix childTransform = compoundShape.GetChildTransform(i);
+                                Matrix newChildWorld = orgTransform * childTransform;
+                                collisionObject.WorldTransform = newChildWorld;
+
+                                collisionObject.CollisionShape = childShape;
+                                float frac = _childCollisionAlgorithms[i].CalculateTimeOfImpact(
+                                        collisionObject, otherObject, dispatchInfo, resultOut
+                                );
+
+                                if (frac < hitFraction)
+                                {
+                                        hitFraction = frac;
+                                }
+
+                                collisionObject.CollisionShape = orgShape;
+                                collisionObject.WorldTransform = orgTransform;
+                        }
+
+                        return hitFraction;
+                }
+
+                public class CreateFunc : CollisionAlgorithmCreateFunction
+                {
+                        public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
+                        {
+                                return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false);
+                        }
+                };
+
+                public class SwappedCreateFunc : CollisionAlgorithmCreateFunction
+                {
+                        public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
+                        {
+                                return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true);
+                        }
+                };
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs
index c4949f0..39438e4 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs
@@ -1,189 +1,189 @@
-/*

-  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 ConvexConcaveCollisionAlgorithm : CollisionAlgorithm

-        {

-                private bool _isSwapped;

-                private ConvexTriangleCallback _convexTriangleCallback;

-

-                public ConvexConcaveCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped)

-                        : base(collisionAlgorithmConstructionInfo)

-                {

-                        _isSwapped = isSwapped;

-                        _convexTriangleCallback = new ConvexTriangleCallback(collisionAlgorithmConstructionInfo.Dispatcher, bodyA, bodyB, isSwapped);

-                }

-

-                public void ClearCache()

-                {

-                        _convexTriangleCallback.ClearCache();

-                }

-

-                public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)

-                {

-                        CollisionObject convexBody = _isSwapped ? bodyB : bodyA;

-                        CollisionObject triBody = _isSwapped ? bodyA : bodyB;

-

-                        if (triBody.CollisionShape.IsConcave)

-                        {

-                                CollisionObject triOb = triBody;

-                                ConcaveShape concaveShape = triOb.CollisionShape as ConcaveShape;

-

-                                if (convexBody.CollisionShape.IsConvex)

-                                {

-                                        float collisionMarginTriangle = concaveShape.Margin;

-

-                                        resultOut.SetPersistentManifold(_convexTriangleCallback.Manifold);

-                                        _convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut);

-

-                                        //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.

-                                        //m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);

-

-                                        _convexTriangleCallback.Manifold.SetBodies(convexBody, triBody);

-                                        concaveShape.ProcessAllTriangles(_convexTriangleCallback, _convexTriangleCallback.AabbMin, _convexTriangleCallback.AabbMax);

-                                }

-                        }

-                }

-

-                public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)

-                {

-                        CollisionObject convexbody = _isSwapped ? bodyB : bodyA;

-                        CollisionObject triBody = _isSwapped ? bodyA : bodyB;

-

-

-                        //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)

-

-                        //only perform CCD above a certain threshold, this prevents blocking on the long run

-                        //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...

-                        float squareMot0 = (convexbody.InterpolationWorldTransform.Translation - convexbody.WorldTransform.Translation).LengthSquared();

-                        if (squareMot0 < convexbody.CcdSquareMotionThreshold)

-                        {

-                                return 1;

-                        }

-

-            Matrix triInv = MathHelper.InvertMatrix(triBody.WorldTransform);

-                        Matrix convexFromLocal = triInv * convexbody.WorldTransform;

-                        Matrix convexToLocal = triInv * convexbody.InterpolationWorldTransform;

-

-                        if (triBody.CollisionShape.IsConcave)

-                        {

-                                Vector3 rayAabbMin = convexFromLocal.Translation;

-                                MathHelper.SetMin(ref rayAabbMin, convexToLocal.Translation);

-                                Vector3 rayAabbMax = convexFromLocal.Translation;

-                                MathHelper.SetMax(ref rayAabbMax, convexToLocal.Translation);

-                                float ccdRadius0 = convexbody.CcdSweptSphereRadius;

-                                rayAabbMin -= new Vector3(ccdRadius0, ccdRadius0, ccdRadius0);

-                                rayAabbMax += new Vector3(ccdRadius0, ccdRadius0, ccdRadius0);

-

-                                float curHitFraction = 1f; //is this available?

-                                LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal,

-                                        convexbody.CcdSweptSphereRadius, curHitFraction);

-

-                                raycastCallback.HitFraction = convexbody.HitFraction;

-

-                                CollisionObject concavebody = triBody;

-

-                                ConcaveShape triangleMesh = concavebody.CollisionShape as ConcaveShape;

-

-                                if (triangleMesh != null)

-                                {

-                                        triangleMesh.ProcessAllTriangles(raycastCallback, rayAabbMin, rayAabbMax);

-                                }

-

-                                if (raycastCallback.HitFraction < convexbody.HitFraction)

-                                {

-                                        convexbody.HitFraction = raycastCallback.HitFraction;

-                                        return raycastCallback.HitFraction;

-                                }

-                        }

-

-                        return 1;

-                }

-

-                public class CreateFunc : CollisionAlgorithmCreateFunction

-                {

-                        public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)

-                        {

-                                return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false);

-                        }

-                }

-

-                public class SwappedCreateFunc : CollisionAlgorithmCreateFunction

-                {

-                        public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)

-                        {

-                                return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true);

-                        }

-                }

-

-                private class LocalTriangleSphereCastCallback : ITriangleCallback

-                {

-                        private Matrix _ccdSphereFromTrans;

-                        private Matrix _ccdSphereToTrans;

-                        private Matrix _meshTransform;

-

-                        private float _ccdSphereRadius;

-                        private float _hitFraction;

-

-                        public LocalTriangleSphereCastCallback(Matrix from, Matrix to, float ccdSphereRadius, float hitFraction)

-                        {

-                                _ccdSphereFromTrans = from;

-                                _ccdSphereToTrans = to;

-                                _ccdSphereRadius = ccdSphereRadius;

-                                _hitFraction = hitFraction;

-                        }

-

-                        public Matrix CcdSphereFromTrans { get { return _ccdSphereFromTrans; } set { _ccdSphereFromTrans = value; } }

-                        public Matrix CcdSphereToTrans { get { return _ccdSphereToTrans; } set { _ccdSphereToTrans = value; } }

-                        public Matrix MeshTransform { get { return _meshTransform; } set { _meshTransform = value; } }

-                        public float CcdSphereRadius { get { return _ccdSphereRadius; } set { _ccdSphereRadius = value; } }

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

-

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

-                        {

-                                //do a swept sphere for now

-                                Matrix ident = Matrix.Identity;

-                                CastResult castResult = new CastResult();

-                                castResult.Fraction = _hitFraction;

-                                SphereShape pointShape = new SphereShape(_ccdSphereRadius);

-                                TriangleShape triShape = new TriangleShape(triangle[0], triangle[1], triangle[2]);

-                                VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();

-                                SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, triShape, simplexSolver);

-                                //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);

-                                //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);

-                                //local space?

-

-                                if (convexCaster.CalcTimeOfImpact(_ccdSphereFromTrans, _ccdSphereToTrans,

-                                        ident, ident, castResult))

-                                {

-                                        if (_hitFraction > castResult.Fraction)

-                                                _hitFraction = castResult.Fraction;

-                                }

-                        }

-                }

-        }

-}

+/*
+  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 ConvexConcaveCollisionAlgorithm : CollisionAlgorithm
+        {
+                private bool _isSwapped;
+                private ConvexTriangleCallback _convexTriangleCallback;
+
+                public ConvexConcaveCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped)
+                        : base(collisionAlgorithmConstructionInfo)
+                {
+                        _isSwapped = isSwapped;
+                        _convexTriangleCallback = new ConvexTriangleCallback(collisionAlgorithmConstructionInfo.Dispatcher, bodyA, bodyB, isSwapped);
+                }
+
+                public void ClearCache()
+                {
+                        _convexTriangleCallback.ClearCache();
+                }
+
+                public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
+                {
+                        CollisionObject convexBody = _isSwapped ? bodyB : bodyA;
+                        CollisionObject triBody = _isSwapped ? bodyA : bodyB;
+
+                        if (triBody.CollisionShape.IsConcave)
+                        {
+                                CollisionObject triOb = triBody;
+                                ConcaveShape concaveShape = triOb.CollisionShape as ConcaveShape;
+
+                                if (convexBody.CollisionShape.IsConvex)
+                                {
+                                        float collisionMarginTriangle = concaveShape.Margin;
+
+                                        resultOut.SetPersistentManifold(_convexTriangleCallback.Manifold);
+                                        _convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut);
+
+                                        //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
+                                        //m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);
+
+                                        _convexTriangleCallback.Manifold.SetBodies(convexBody, triBody);
+                                        concaveShape.ProcessAllTriangles(_convexTriangleCallback, _convexTriangleCallback.AabbMin, _convexTriangleCallback.AabbMax);
+                                }
+                        }
+                }
+
+                public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
+                {
+                        CollisionObject convexbody = _isSwapped ? bodyB : bodyA;
+                        CollisionObject triBody = _isSwapped ? bodyA : bodyB;
+
+
+                        //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
+
+                        //only perform CCD above a certain threshold, this prevents blocking on the long run
+                        //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
+                        float squareMot0 = (convexbody.InterpolationWorldTransform.Translation - convexbody.WorldTransform.Translation).LengthSquared();
+                        if (squareMot0 < convexbody.CcdSquareMotionThreshold)
+                        {
+                                return 1;
+                        }
+
+            Matrix triInv = MathHelper.InvertMatrix(triBody.WorldTransform);
+                        Matrix convexFromLocal = triInv * convexbody.WorldTransform;
+                        Matrix convexToLocal = triInv * convexbody.InterpolationWorldTransform;
+
+                        if (triBody.CollisionShape.IsConcave)
+                        {
+                                Vector3 rayAabbMin = convexFromLocal.Translation;
+                                MathHelper.SetMin(ref rayAabbMin, convexToLocal.Translation);
+                                Vector3 rayAabbMax = convexFromLocal.Translation;
+                                MathHelper.SetMax(ref rayAabbMax, convexToLocal.Translation);
+                                float ccdRadius0 = convexbody.CcdSweptSphereRadius;
+                                rayAabbMin -= new Vector3(ccdRadius0, ccdRadius0, ccdRadius0);
+                                rayAabbMax += new Vector3(ccdRadius0, ccdRadius0, ccdRadius0);
+
+                                float curHitFraction = 1f; //is this available?
+                                LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal,
+                                        convexbody.CcdSweptSphereRadius, curHitFraction);
+
+                                raycastCallback.HitFraction = convexbody.HitFraction;
+
+                                CollisionObject concavebody = triBody;
+
+                                ConcaveShape triangleMesh = concavebody.CollisionShape as ConcaveShape;
+
+                                if (triangleMesh != null)
+                                {
+                                        triangleMesh.ProcessAllTriangles(raycastCallback, rayAabbMin, rayAabbMax);
+                                }
+
+                                if (raycastCallback.HitFraction < convexbody.HitFraction)
+                                {
+                                        convexbody.HitFraction = raycastCallback.HitFraction;
+                                        return raycastCallback.HitFraction;
+                                }
+                        }
+
+                        return 1;
+                }
+
+                public class CreateFunc : CollisionAlgorithmCreateFunction
+                {
+                        public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
+                        {
+                                return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false);
+                        }
+                }
+
+                public class SwappedCreateFunc : CollisionAlgorithmCreateFunction
+                {
+                        public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
+                        {
+                                return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true);
+                        }
+                }
+
+                private class LocalTriangleSphereCastCallback : ITriangleCallback
+                {
+                        private Matrix _ccdSphereFromTrans;
+                        private Matrix _ccdSphereToTrans;
+                        private Matrix _meshTransform;
+
+                        private float _ccdSphereRadius;
+                        private float _hitFraction;
+
+                        public LocalTriangleSphereCastCallback(Matrix from, Matrix to, float ccdSphereRadius, float hitFraction)
+                        {
+                                _ccdSphereFromTrans = from;
+                                _ccdSphereToTrans = to;
+                                _ccdSphereRadius = ccdSphereRadius;
+                                _hitFraction = hitFraction;
+                        }
+
+                        public Matrix CcdSphereFromTrans { get { return _ccdSphereFromTrans; } set { _ccdSphereFromTrans = value; } }
+                        public Matrix CcdSphereToTrans { get { return _ccdSphereToTrans; } set { _ccdSphereToTrans = value; } }
+                        public Matrix MeshTransform { get { return _meshTransform; } set { _meshTransform = value; } }
+                        public float CcdSphereRadius { get { return _ccdSphereRadius; } set { _ccdSphereRadius = value; } }
+                        public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } }
+
+                        public void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex)
+                        {
+                                //do a swept sphere for now
+                                Matrix ident = Matrix.Identity;
+                                CastResult castResult = new CastResult();
+                                castResult.Fraction = _hitFraction;
+                                SphereShape pointShape = new SphereShape(_ccdSphereRadius);
+                                TriangleShape triShape = new TriangleShape(triangle[0], triangle[1], triangle[2]);
+                                VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
+                                SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, triShape, simplexSolver);
+                                //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
+                                //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
+                                //local space?
+
+                                if (convexCaster.CalcTimeOfImpact(_ccdSphereFromTrans, _ccdSphereToTrans,
+                                        ident, ident, castResult))
+                                {
+                                        if (_hitFraction > castResult.Fraction)
+                                                _hitFraction = castResult.Fraction;
+                                }
+                        }
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs
index 8fa4837..01c8fe0 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs
@@ -1,193 +1,193 @@
-/*

-  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;

-

-namespace XnaDevRu.BulletX

-{

-        public class ConvexConvexAlgorithm : CollisionAlgorithm, IDisposable

-        {

-                private const bool DisableCcd = false;

-                private GjkPairDetector _gjkPairDetector;

-                private bool _ownManifold;

-                private PersistentManifold _manifold;

-                private bool _lowLevelOfDetail;

-

-                public ConvexConvexAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)

-                        : base(collisionAlgorithmConstructionInfo)

-                {

-                        _gjkPairDetector = new GjkPairDetector(null, null, simplexSolver, penetrationDepthSolver);

-                        _ownManifold = false;

-                        _manifold = manifold;

-                        _lowLevelOfDetail = false;

-                }

-

-                public bool LowLevelOfDetail { get { return _lowLevelOfDetail; } set { _lowLevelOfDetail = value; } }

-                public bool OwnManifold { get { return _ownManifold; } set { _ownManifold = value; } }

-                public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } }

-

-                public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)

-                {

-                        if (_manifold == null)

-                        {

-                                //swapped?

-                                _manifold = Dispatcher.GetNewManifold(bodyA, bodyB);

-                                _ownManifold = true;

-                        }

-                        resultOut.SetPersistentManifold(_manifold);

-

-                        ConvexShape min0 = bodyA.CollisionShape as ConvexShape;

-                        ConvexShape min1 = bodyB.CollisionShape as ConvexShape;

-

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

-

-                        //TODO: if (dispatchInfo.m_useContinuous)

-                        _gjkPairDetector.setMinkowskiA(min0);

-                        _gjkPairDetector.setMinkowskiB(min1);

-                        input.MaximumDistanceSquared = min0.Margin + min1.Margin + PersistentManifold.ContactBreakingThreshold;

-                        input.MaximumDistanceSquared *= input.MaximumDistanceSquared;

-

-                        //      input.m_maximumDistanceSquared = 1e30f;

-

-                        input.TransformA = bodyA.WorldTransform;

-                        input.TransformB = bodyB.WorldTransform;

-

-                        _gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.DebugDraw);

-                }

-

-                public override float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)

-                {

-                        //Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold

-

-                        //Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold

-                        //col0->m_worldTransform,

-                        float resultFraction = 1f;

-

-                        float squareMotA = (colA.InterpolationWorldTransform.Translation - colA.WorldTransform.Translation).LengthSquared();

-                        float squareMotB = (colB.InterpolationWorldTransform.Translation - colB.WorldTransform.Translation).LengthSquared();

-

-                        if (squareMotA < colA.CcdSquareMotionThreshold &&

-                                squareMotB < colB.CcdSquareMotionThreshold)

-                                return resultFraction;

-

-                        if (DisableCcd)

-                                return 1f;

-

-                        //An adhoc way of testing the Continuous Collision Detection algorithms

-                        //One object is approximated as a sphere, to simplify things

-                        //Starting in penetration should report no time of impact

-                        //For proper CCD, better accuracy and handling of 'allowed' penetration should be added

-                        //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)

-

-                        // Convex0 against sphere for Convex1

-                        {

-                                ConvexShape convexA = colA.CollisionShape as ConvexShape;

-

-                                SphereShape sphereB = new SphereShape(colB.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation

-                                CastResult result = new CastResult();

-                                VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();

-                                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);

-                                //Simplification, one object is simplified as a sphere

-                                GjkConvexCast ccdB = new GjkConvexCast(convexA, sphereB, voronoiSimplex);

-                                //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);

-                                if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,

-                                        colB.WorldTransform, colB.InterpolationWorldTransform, result))

-                                {

-                                        //store result.m_fraction in both bodies

-                                        if (colA.HitFraction > result.Fraction)

-                                                colA.HitFraction = result.Fraction;

-

-                                        if (colB.HitFraction > result.Fraction)

-                                                colB.HitFraction = result.Fraction;

-

-                                        if (resultFraction > result.Fraction)

-                                                resultFraction = result.Fraction;

-                                }

-                        }

-

-                        // Sphere (for convex0) against Convex1

-                        {

-                                ConvexShape convexB = colB.CollisionShape as ConvexShape;

-

-                                SphereShape sphereA = new SphereShape(colA.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation

-                                CastResult result = new CastResult();

-                                VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();

-                                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);

-                                ///Simplification, one object is simplified as a sphere

-                                GjkConvexCast ccdB = new GjkConvexCast(sphereA, convexB, voronoiSimplex);

-                                //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);

-                                if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,

-                                        colB.WorldTransform, colB.InterpolationWorldTransform, result))

-                                {

-                                        //store result.m_fraction in both bodies

-                                        if (colA.HitFraction > result.Fraction)

-                                                colA.HitFraction = result.Fraction;

-

-                                        if (colB.HitFraction > result.Fraction)

-                                                colB.HitFraction = result.Fraction;

-

-                                        if (resultFraction > result.Fraction)

-                                                resultFraction = result.Fraction;

-                                }

-                        }

-                        return resultFraction;

-                }

-

-                public class CreateFunc : CollisionAlgorithmCreateFunction

-                {

-                        private IConvexPenetrationDepthSolver _penetrationDepthSolver;

-                        private ISimplexSolver _simplexSolver;

-                        //private bool _ownsSolvers;

-

-                        public CreateFunc()

-                        {

-                                //_ownsSolvers = true;

-                                _simplexSolver = new VoronoiSimplexSolver();

-                                _penetrationDepthSolver = new GjkEpaPenetrationDepthSolver();

-                        }

-

-                        public CreateFunc(ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)

-                        {

-                                //_ownsSolvers = false;

-                                _simplexSolver = simplexSolver;

-                                _penetrationDepthSolver = penetrationDepthSolver;

-                        }

-

-                        public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)

-                        {

-                                return new ConvexConvexAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, _simplexSolver, _penetrationDepthSolver);

-                        }

-                }

-

-                #region IDisposable Members

-                public void Dispose()

-                {

-                        if (_ownManifold)

-                        {

-                                if (_manifold != null)

-                                        Dispatcher.ReleaseManifold(_manifold);

-                        }

-                }

-                #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;
+
+namespace XnaDevRu.BulletX
+{
+        public class ConvexConvexAlgorithm : CollisionAlgorithm, IDisposable
+        {
+                private const bool DisableCcd = false;
+                private GjkPairDetector _gjkPairDetector;
+                private bool _ownManifold;
+                private PersistentManifold _manifold;
+                private bool _lowLevelOfDetail;
+
+                public ConvexConvexAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
+                        : base(collisionAlgorithmConstructionInfo)
+                {
+                        _gjkPairDetector = new GjkPairDetector(null, null, simplexSolver, penetrationDepthSolver);
+                        _ownManifold = false;
+                        _manifold = manifold;
+                        _lowLevelOfDetail = false;
+                }
+
+                public bool LowLevelOfDetail { get { return _lowLevelOfDetail; } set { _lowLevelOfDetail = value; } }
+                public bool OwnManifold { get { return _ownManifold; } set { _ownManifold = value; } }
+                public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } }
+
+                public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
+                {
+                        if (_manifold == null)
+                        {
+                                //swapped?
+                                _manifold = Dispatcher.GetNewManifold(bodyA, bodyB);
+                                _ownManifold = true;
+                        }
+                        resultOut.SetPersistentManifold(_manifold);
+
+                        ConvexShape min0 = bodyA.CollisionShape as ConvexShape;
+                        ConvexShape min1 = bodyB.CollisionShape as ConvexShape;
+
+                        GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
+
+                        //TODO: if (dispatchInfo.m_useContinuous)
+                        _gjkPairDetector.setMinkowskiA(min0);
+                        _gjkPairDetector.setMinkowskiB(min1);
+                        input.MaximumDistanceSquared = min0.Margin + min1.Margin + PersistentManifold.ContactBreakingThreshold;
+                        input.MaximumDistanceSquared *= input.MaximumDistanceSquared;
+
+                        //      input.m_maximumDistanceSquared = 1e30f;
+
+                        input.TransformA = bodyA.WorldTransform;
+                        input.TransformB = bodyB.WorldTransform;
+
+                        _gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.DebugDraw);
+                }
+
+                public override float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
+                {
+                        //Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
+
+                        //Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
+                        //col0->m_worldTransform,
+                        float resultFraction = 1f;
+
+                        float squareMotA = (colA.InterpolationWorldTransform.Translation - colA.WorldTransform.Translation).LengthSquared();
+                        float squareMotB = (colB.InterpolationWorldTransform.Translation - colB.WorldTransform.Translation).LengthSquared();
+
+                        if (squareMotA < colA.CcdSquareMotionThreshold &&
+                                squareMotB < colB.CcdSquareMotionThreshold)
+                                return resultFraction;
+
+                        if (DisableCcd)
+                                return 1f;
+
+                        //An adhoc way of testing the Continuous Collision Detection algorithms
+                        //One object is approximated as a sphere, to simplify things
+                        //Starting in penetration should report no time of impact
+                        //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
+                        //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
+
+                        // Convex0 against sphere for Convex1
+                        {
+                                ConvexShape convexA = colA.CollisionShape as ConvexShape;
+
+                                SphereShape sphereB = new SphereShape(colB.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
+                                CastResult result = new CastResult();
+                                VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
+                                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+                                //Simplification, one object is simplified as a sphere
+                                GjkConvexCast ccdB = new GjkConvexCast(convexA, sphereB, voronoiSimplex);
+                                //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
+                                if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,
+                                        colB.WorldTransform, colB.InterpolationWorldTransform, result))
+                                {
+                                        //store result.m_fraction in both bodies
+                                        if (colA.HitFraction > result.Fraction)
+                                                colA.HitFraction = result.Fraction;
+
+                                        if (colB.HitFraction > result.Fraction)
+                                                colB.HitFraction = result.Fraction;
+
+                                        if (resultFraction > result.Fraction)
+                                                resultFraction = result.Fraction;
+                                }
+                        }
+
+                        // Sphere (for convex0) against Convex1
+                        {
+                                ConvexShape convexB = colB.CollisionShape as ConvexShape;
+
+                                SphereShape sphereA = new SphereShape(colA.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
+                                CastResult result = new CastResult();
+                                VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
+                                //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
+                                ///Simplification, one object is simplified as a sphere
+                                GjkConvexCast ccdB = new GjkConvexCast(sphereA, convexB, voronoiSimplex);
+                                //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
+                                if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,
+                                        colB.WorldTransform, colB.InterpolationWorldTransform, result))
+                                {
+                                        //store result.m_fraction in both bodies
+                                        if (colA.HitFraction > result.Fraction)
+                                                colA.HitFraction = result.Fraction;
+
+                                        if (colB.HitFraction > result.Fraction)
+                                                colB.HitFraction = result.Fraction;
+
+                                        if (resultFraction > result.Fraction)
+                                                resultFraction = result.Fraction;
+                                }
+                        }
+                        return resultFraction;
+                }
+
+                public class CreateFunc : CollisionAlgorithmCreateFunction
+                {
+                        private IConvexPenetrationDepthSolver _penetrationDepthSolver;
+                        private ISimplexSolver _simplexSolver;
+                        //private bool _ownsSolvers;
+
+                        public CreateFunc()
+                        {
+                                //_ownsSolvers = true;
+                                _simplexSolver = new VoronoiSimplexSolver();
+                                _penetrationDepthSolver = new GjkEpaPenetrationDepthSolver();
+                        }
+
+                        public CreateFunc(ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
+                        {
+                                //_ownsSolvers = false;
+                                _simplexSolver = simplexSolver;
+                                _penetrationDepthSolver = penetrationDepthSolver;
+                        }
+
+                        public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
+                        {
+                                return new ConvexConvexAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, _simplexSolver, _penetrationDepthSolver);
+                        }
+                }
+
+                #region IDisposable Members
+                public void Dispose()
+                {
+                        if (_ownManifold)
+                        {
+                                if (_manifold != null)
+                                        Dispatcher.ReleaseManifold(_manifold);
+                        }
+                }
+                #endregion
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs
index 5355817..ba21c72 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs
@@ -1,130 +1,130 @@
-/*

-  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 ConvexTriangleCallback : ITriangleCallback, IDisposable

-        {

-                private CollisionObject _convexBody;

-                private CollisionObject _triBody;

-

-                private Vector3 _aabbMin;

-                private Vector3 _aabbMax;

-

-                private ManifoldResult _resultOut;

-

-                private IDispatcher _dispatcher;

-                private DispatcherInfo _dispatchInfo;

-                private float _collisionMarginTriangle;

-

-                private int _triangleCount;

-

-                private PersistentManifold _manifold;

-

-                public ConvexTriangleCallback(IDispatcher dispatcher, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped)

-                {

-                        _dispatcher = dispatcher;

-                        _dispatchInfo = null;

-                        _convexBody = isSwapped ? bodyB : bodyA;

-                        _triBody = isSwapped ? bodyA : bodyB;

-

-                        // create the manifold from the dispatcher 'manifold pool'

-                        _manifold = _dispatcher.GetNewManifold(_convexBody, _triBody);

-                        ClearCache();

-                }

-

-                public Vector3 AabbMin { get { return _aabbMin; } }

-                public Vector3 AabbMax { get { return _aabbMax; } }

-                public int TriangleCount { get { return _triangleCount; } set { _triangleCount = value; } }

-                public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } }

-

-                public void SetTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut)

-                {

-                        _dispatchInfo = dispatchInfo;

-                        _collisionMarginTriangle = collisionMarginTriangle;

-                        _resultOut = resultOut;

-

-                        //recalc aabbs

-            Matrix convexInTriangleSpace = MathHelper.InvertMatrix(_triBody.WorldTransform) * _convexBody.WorldTransform;

-                        CollisionShape convexShape = _convexBody.CollisionShape;

-                        //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);

-                        convexShape.GetAabb(convexInTriangleSpace, out _aabbMin, out _aabbMax);

-                        float extraMargin = collisionMarginTriangle;

-                        Vector3 extra = new Vector3(extraMargin, extraMargin, extraMargin);

-

-                        _aabbMax += extra;

-                        _aabbMin -= extra;

-                }

-

-                public void ClearCache()

-                {

-                        _dispatcher.ClearManifold(_manifold);

-                }

-

-                #region ITriangleCallback Members

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

-                {

-                        //aabb filter is already applied!       

-                        CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo();

-                        collisionAlgorithmConstructionInfo.Dispatcher = _dispatcher;

-

-                        CollisionObject collisionObject = _triBody;

-

-                        //debug drawing of the overlapping triangles

-                        /*if (m_dispatchInfoPtr && m_dispatchInfoPtr.m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0)

-                        {

-                                Vector3 color = new Vector3(255, 255, 0);

-                                btTransform & tr = ob->WorldTransform;

-                                m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]), tr(triangle[1]), color);

-                                m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]), tr(triangle[2]), color);

-                                m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]), tr(triangle[0]), color);

-                        }*/

-

-                        if (_convexBody.CollisionShape.IsConvex)

-                        {

-                                TriangleShape triangleShape = new TriangleShape(triangle[0], triangle[1], triangle[2]);

-                                triangleShape.Margin=_collisionMarginTriangle;

-

-                                CollisionShape tempShape = collisionObject.CollisionShape;

-                                collisionObject.CollisionShape = triangleShape;

-

-                                CollisionAlgorithm collisionAlgorithm = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(_convexBody, _triBody, _manifold);

-

-                                _resultOut.SetShapeIdentifiers(-1, -1, partID, triangleIndex);

-                                collisionAlgorithm.ProcessCollision(_convexBody, _triBody, _dispatchInfo, _resultOut);

-                                collisionObject.CollisionShape = tempShape;

-                        }

-                }

-                #endregion

-                #region IDisposable Members

-                public void Dispose()

-                {

-                        ClearCache();

-                        _dispatcher.ReleaseManifold(_manifold);

-                }

-                #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 class ConvexTriangleCallback : ITriangleCallback, IDisposable
+        {
+                private CollisionObject _convexBody;
+                private CollisionObject _triBody;
+
+                private Vector3 _aabbMin;
+                private Vector3 _aabbMax;
+
+                private ManifoldResult _resultOut;
+
+                private IDispatcher _dispatcher;
+                private DispatcherInfo _dispatchInfo;
+                private float _collisionMarginTriangle;
+
+                private int _triangleCount;
+
+                private PersistentManifold _manifold;
+
+                public ConvexTriangleCallback(IDispatcher dispatcher, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped)
+                {
+                        _dispatcher = dispatcher;
+                        _dispatchInfo = null;
+                        _convexBody = isSwapped ? bodyB : bodyA;
+                        _triBody = isSwapped ? bodyA : bodyB;
+
+                        // create the manifold from the dispatcher 'manifold pool'
+                        _manifold = _dispatcher.GetNewManifold(_convexBody, _triBody);
+                        ClearCache();
+                }
+
+                public Vector3 AabbMin { get { return _aabbMin; } }
+                public Vector3 AabbMax { get { return _aabbMax; } }
+                public int TriangleCount { get { return _triangleCount; } set { _triangleCount = value; } }
+                public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } }
+
+                public void SetTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
+                {
+                        _dispatchInfo = dispatchInfo;
+                        _collisionMarginTriangle = collisionMarginTriangle;
+                        _resultOut = resultOut;
+
+                        //recalc aabbs
+            Matrix convexInTriangleSpace = MathHelper.InvertMatrix(_triBody.WorldTransform) * _convexBody.WorldTransform;
+                        CollisionShape convexShape = _convexBody.CollisionShape;
+                        //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
+                        convexShape.GetAabb(convexInTriangleSpace, out _aabbMin, out _aabbMax);
+                        float extraMargin = collisionMarginTriangle;
+                        Vector3 extra = new Vector3(extraMargin, extraMargin, extraMargin);
+
+                        _aabbMax += extra;
+                        _aabbMin -= extra;
+                }
+
+                public void ClearCache()
+                {
+                        _dispatcher.ClearManifold(_manifold);
+                }
+
+                #region ITriangleCallback Members
+                public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex)
+                {
+                        //aabb filter is already applied!       
+                        CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo();
+                        collisionAlgorithmConstructionInfo.Dispatcher = _dispatcher;
+
+                        CollisionObject collisionObject = _triBody;
+
+                        //debug drawing of the overlapping triangles
+                        /*if (m_dispatchInfoPtr && m_dispatchInfoPtr.m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0)
+                        {
+                                Vector3 color = new Vector3(255, 255, 0);
+                                btTransform & tr = ob->WorldTransform;
+                                m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]), tr(triangle[1]), color);
+                                m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]), tr(triangle[2]), color);
+                                m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]), tr(triangle[0]), color);
+                        }*/
+
+                        if (_convexBody.CollisionShape.IsConvex)
+                        {
+                                TriangleShape triangleShape = new TriangleShape(triangle[0], triangle[1], triangle[2]);
+                                triangleShape.Margin=_collisionMarginTriangle;
+
+                                CollisionShape tempShape = collisionObject.CollisionShape;
+                                collisionObject.CollisionShape = triangleShape;
+
+                                CollisionAlgorithm collisionAlgorithm = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(_convexBody, _triBody, _manifold);
+
+                                _resultOut.SetShapeIdentifiers(-1, -1, partID, triangleIndex);
+                                collisionAlgorithm.ProcessCollision(_convexBody, _triBody, _dispatchInfo, _resultOut);
+                                collisionObject.CollisionShape = tempShape;
+                        }
+                }
+                #endregion
+                #region IDisposable Members
+                public void Dispose()
+                {
+                        ClearCache();
+                        _dispatcher.ReleaseManifold(_manifold);
+                }
+                #endregion
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs
index 13cb3d9..5995e2c 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs
@@ -1,52 +1,52 @@
-/*

-  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;

-

-namespace XnaDevRu.BulletX

-{

-        /// <summary>

-        /// EmptyAlgorithm is a stub for unsupported collision pairs.

-        /// The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame.

-        /// </summary>

-        public class EmptyAlgorithm : CollisionAlgorithm

-        {

-                public EmptyAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo)

-                        : base(collisionAlgorithmConstructionInfo) { }

-

-                public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { }

-

-                public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)

-                {

-                        return 1f;

-                }

-

-                public class CreateFunc : CollisionAlgorithmCreateFunction

-                {

-                        public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)

-                        {

-                                return new EmptyAlgorithm(collisionAlgorithmConstructionInfo);

-                        }

-                };

-        }

-}

+/*
+  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;
+
+namespace XnaDevRu.BulletX
+{
+        /// <summary>
+        /// EmptyAlgorithm is a stub for unsupported collision pairs.
+        /// The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame.
+        /// </summary>
+        public class EmptyAlgorithm : CollisionAlgorithm
+        {
+                public EmptyAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo)
+                        : base(collisionAlgorithmConstructionInfo) { }
+
+                public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { }
+
+                public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
+                {
+                        return 1f;
+                }
+
+                public class CreateFunc : CollisionAlgorithmCreateFunction
+                {
+                        public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
+                        {
+                                return new EmptyAlgorithm(collisionAlgorithmConstructionInfo);
+                        }
+                };
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs
index 63f37ea..69143a1 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs
@@ -1,147 +1,147 @@
-/*

-  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 ContactAddedCallback(ManifoldPoint contactPoint, CollisionObject collisionObjectA, int partIdA, int indexA, CollisionObject collisionObjectB, int partIdB, int indexB);

-

-        public class ManifoldResult : DiscreteCollisionDetectorInterface.Result

-        {

-                private PersistentManifold _manifold;

-                private static ContactAddedCallback _contactAddedCallback = null;

-

-                //we need this for compounds

-                private Matrix _rootTransA;

-                private Matrix _rootTransB;

-

-                private CollisionObject _bodyA;

-                private CollisionObject _bodyB;

-                private int _partIdA;

-                private int _partIdB;

-                private int _indexA;

-                private int _indexB;

-

-                public ManifoldResult()

-                {

-                }

-

-                public ManifoldResult(CollisionObject bodyA, CollisionObject bodyB)

-                {

-                        _bodyA = bodyA;

-                        _bodyB = bodyB;

-                        _rootTransA = bodyA.WorldTransform;

-                        _rootTransB = bodyB.WorldTransform;

-                }

-

-                public static ContactAddedCallback ContactAddedCallback { get { return _contactAddedCallback; } set { _contactAddedCallback = value; } }

-

-                public void SetPersistentManifold(PersistentManifold manifold)

-                {

-                        _manifold = manifold;

-                }

-

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

-                {

-                        _partIdA = partIdA;

-                        _partIdB = partIdB;

-                        _indexA = indexA;

-                        _indexB = indexB;

-                }

-

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

-                {

-                        if (_manifold == null)

-                                throw new BulletException("Manifold Pointer is null.");

-

-                        //order in manifold needs to match

-

-                        if (depth > PersistentManifold.ContactBreakingThreshold)

-                                return;

-

-                        bool isSwapped = _manifold.BodyA != _bodyA;

-

-                        Vector3 pointA = pointInWorld + normalOnBInWorld * depth;

-                        Vector3 localA;

-                        Vector3 localB;

-

-                        if (isSwapped)

-                        {

-                                localA = MathHelper.InvXForm(_rootTransB, pointA);

-                                localB = MathHelper.InvXForm(_rootTransA, pointInWorld);

-                        }

-                        else

-                        {

-                                localA = MathHelper.InvXForm(_rootTransA, pointA);

-                                localB = MathHelper.InvXForm(_rootTransB, pointInWorld);

-                        }

-

-                        ManifoldPoint newPt = new ManifoldPoint(localA, localB, normalOnBInWorld, depth);

-

-                        int insertIndex = _manifold.GetCacheEntry(newPt);

-

-                        newPt.CombinedFriction = CalculateCombinedFriction(_bodyA, _bodyB);

-                        newPt.CombinedRestitution = CalculateCombinedRestitution(_bodyA, _bodyB);

-

-                        //User can override friction and/or restitution

-                        if (_contactAddedCallback != null &&

-                                //and if either of the two bodies requires custom material

-                                 ((_bodyA.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0 ||

-                                   (_bodyB.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0))

-                        {

-                                //experimental feature info, for per-triangle material etc.

-                                CollisionObject obj0 = isSwapped ? _bodyB : _bodyA;

-                                CollisionObject obj1 = isSwapped ? _bodyA : _bodyB;

-                                _contactAddedCallback(newPt, obj0, _partIdA, _indexA, obj1, _partIdB, _indexB);

-                        }

-

-                        if (insertIndex >= 0)

-                        {

-                                _manifold.ReplaceContactPoint(newPt, insertIndex);

-                        }

-                        else

-                        {

-                                _manifold.AddManifoldPoint(newPt);

-                        }

-                }

-

-                private float CalculateCombinedFriction(CollisionObject bodyA, CollisionObject bodyB)

-                {

-                        float friction = bodyA.Friction * bodyB.Friction;

-

-                        float MaxFriction = 10;

-                        if (friction < -MaxFriction)

-                                friction = -MaxFriction;

-                        if (friction > MaxFriction)

-                                friction = MaxFriction;

-                        return friction;

-                }

-

-                private float CalculateCombinedRestitution(CollisionObject bodyA, CollisionObject bodyB)

-                {

-                        return bodyA.Restitution * bodyB.Restitution;

-                }

-        }

-}

+/*
+  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 ContactAddedCallback(ManifoldPoint contactPoint, CollisionObject collisionObjectA, int partIdA, int indexA, CollisionObject collisionObjectB, int partIdB, int indexB);
+
+        public class ManifoldResult : DiscreteCollisionDetectorInterface.Result
+        {
+                private PersistentManifold _manifold;
+                private static ContactAddedCallback _contactAddedCallback = null;
+
+                //we need this for compounds
+                private Matrix _rootTransA;
+                private Matrix _rootTransB;
+
+                private CollisionObject _bodyA;
+                private CollisionObject _bodyB;
+                private int _partIdA;
+                private int _partIdB;
+                private int _indexA;
+                private int _indexB;
+
+                public ManifoldResult()
+                {
+                }
+
+                public ManifoldResult(CollisionObject bodyA, CollisionObject bodyB)
+                {
+                        _bodyA = bodyA;
+                        _bodyB = bodyB;
+                        _rootTransA = bodyA.WorldTransform;
+                        _rootTransB = bodyB.WorldTransform;
+                }
+
+                public static ContactAddedCallback ContactAddedCallback { get { return _contactAddedCallback; } set { _contactAddedCallback = value; } }
+
+                public void SetPersistentManifold(PersistentManifold manifold)
+                {
+                        _manifold = manifold;
+                }
+
+                public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB)
+                {
+                        _partIdA = partIdA;
+                        _partIdB = partIdB;
+                        _indexA = indexA;
+                        _indexB = indexB;
+                }
+
+        public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth)
+                {
+                        if (_manifold == null)
+                                throw new BulletException("Manifold Pointer is null.");
+
+                        //order in manifold needs to match
+
+                        if (depth > PersistentManifold.ContactBreakingThreshold)
+                                return;
+
+                        bool isSwapped = _manifold.BodyA != _bodyA;
+
+                        Vector3 pointA = pointInWorld + normalOnBInWorld * depth;
+                        Vector3 localA;
+                        Vector3 localB;
+
+                        if (isSwapped)
+                        {
+                                localA = MathHelper.InvXForm(_rootTransB, pointA);
+                                localB = MathHelper.InvXForm(_rootTransA, pointInWorld);
+                        }
+                        else
+                        {
+                                localA = MathHelper.InvXForm(_rootTransA, pointA);
+                                localB = MathHelper.InvXForm(_rootTransB, pointInWorld);
+                        }
+
+                        ManifoldPoint newPt = new ManifoldPoint(localA, localB, normalOnBInWorld, depth);
+
+                        int insertIndex = _manifold.GetCacheEntry(newPt);
+
+                        newPt.CombinedFriction = CalculateCombinedFriction(_bodyA, _bodyB);
+                        newPt.CombinedRestitution = CalculateCombinedRestitution(_bodyA, _bodyB);
+
+                        //User can override friction and/or restitution
+                        if (_contactAddedCallback != null &&
+                                //and if either of the two bodies requires custom material
+                                 ((_bodyA.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0 ||
+                                   (_bodyB.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0))
+                        {
+                                //experimental feature info, for per-triangle material etc.
+                                CollisionObject obj0 = isSwapped ? _bodyB : _bodyA;
+                                CollisionObject obj1 = isSwapped ? _bodyA : _bodyB;
+                                _contactAddedCallback(newPt, obj0, _partIdA, _indexA, obj1, _partIdB, _indexB);
+                        }
+
+                        if (insertIndex >= 0)
+                        {
+                                _manifold.ReplaceContactPoint(newPt, insertIndex);
+                        }
+                        else
+                        {
+                                _manifold.AddManifoldPoint(newPt);
+                        }
+                }
+
+                private float CalculateCombinedFriction(CollisionObject bodyA, CollisionObject bodyB)
+                {
+                        float friction = bodyA.Friction * bodyB.Friction;
+
+                        float MaxFriction = 10;
+                        if (friction < -MaxFriction)
+                                friction = -MaxFriction;
+                        if (friction > MaxFriction)
+                                friction = MaxFriction;
+                        return friction;
+                }
+
+                private float CalculateCombinedRestitution(CollisionObject bodyA, CollisionObject bodyB)
+                {
+                        return bodyA.Restitution * bodyB.Restitution;
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs
index 957843b..39eb53a 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs
@@ -1,304 +1,304 @@
-/*

-  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 SimulationIslandManager

-        {

-                private UnionFind _unionFind = new UnionFind();

-

-                public void InitUnionFind(int n)

-                {

-                        _unionFind.Reset(n);

-                }

-

-                public UnionFind UnionFind { get { return _unionFind; } }

-

-                public virtual void UpdateActivationState(CollisionWorld world, IDispatcher dispatcher)

-                {

-                        InitUnionFind(world.CollisionObjectsCount);

-

-                        // put the index into m_controllers into m_tag  

-                        int index = 0;

-                        for (int i = 0; i < world.CollisionObjects.Count; i++)

-                        {

-                                world.CollisionObjects[i].IslandTag = index;

-                                world.CollisionObjects[i].HitFraction = 1;

-                                world.CollisionObjects[i].CompanionID = -1;

-                                index++;

-                        }

-                        // do the union find

-                        FindUnions(dispatcher);

-                }

-

-                public virtual void StoreIslandActivationState(CollisionWorld world)

-                {

-                        // put the islandId ('find' value) into m_tag                   

-                        int index = 0;

-                        for (int i = 0; i < world.CollisionObjects.Count; i++)

-                        {

-                                if (world.CollisionObjects[i].MergesSimulationIslands)

-                                {

-                                        world.CollisionObjects[i].IslandTag = _unionFind.Find(index);

-                                        world.CollisionObjects[i].CompanionID = -1;

-                                }

-                                else

-                                {

-                                        world.CollisionObjects[i].IslandTag = -1;

-                                        world.CollisionObjects[i].CompanionID = -2;

-                                }

-                                index++;

-                        }

-                }

-

-                public void FindUnions(IDispatcher dispatcher)

-                {

-                        for (int i = 0; i < dispatcher.ManifoldCount; i++)

-                        {

-                                PersistentManifold manifold = dispatcher.GetManifoldByIndex(i);

-                                //static objects (invmass 0.f) don't merge !

-

-                                CollisionObject colObjA = manifold.BodyA as CollisionObject;

-                                CollisionObject colObjB = manifold.BodyB as CollisionObject;

-

-                                if (((colObjA != null) && (colObjA.MergesSimulationIslands)) &&

-                                        ((colObjB != null) && (colObjB.MergesSimulationIslands)))

-                                {

-                                        _unionFind.Unite(colObjA.IslandTag, colObjB.IslandTag);

-                                }

-                        }

-                }

-

-                public void BuildAndProcessIslands(IDispatcher dispatcher, List<CollisionObject> collisionObjects, IIslandCallback callback)

-                {

-                        //we are going to sort the unionfind array, and store the element id in the size

-                        //afterwards, we clean unionfind, to make sure no-one uses it anymore

-                        UnionFind.SortIslands();

-                        int numElem = UnionFind.ElementCount;

-

-                        int endIslandIndex = 1;

-                        int startIslandIndex;

-

-                        //update the sleeping state for bodies, if all are sleeping

-                        for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)

-                        {

-                                int islandId = UnionFind[startIslandIndex].ID;

-                                for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++)

-                                {

-                                }

-

-                                //int numSleeping = 0;

-

-                                bool allSleeping = true;

-

-                                int idx;

-                                for (idx = startIslandIndex; idx < endIslandIndex; idx++)

-                                {

-                                        int i = UnionFind[idx].Size;

-

-                                        CollisionObject colObjA = collisionObjects[i];

-                                        if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1))

-                                        {

-                                                Console.WriteLine("error in island management");

-                                        }

-

-                                        BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1));

-                                        if (colObjA.IslandTag == islandId)

-                                        {

-                                                if (colObjA.ActivationState == ActivationState.Active)

-                                                {

-                                                        allSleeping = false;

-                                                }

-                                                if (colObjA.ActivationState == ActivationState.DisableDeactivation)

-                                                {

-                                                        allSleeping = false;

-                                                }

-                                        }

-                                }

-

-

-                                if (allSleeping)

-                                {

-                                        for (idx = startIslandIndex; idx < endIslandIndex; idx++)

-                                        {

-                                                int i = UnionFind[idx].Size;

-                                                CollisionObject colObjA = collisionObjects[i];

-                                                if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1))

-                                                {

-                                                        Console.WriteLine("error in island management");

-                                                }

-

-                                                BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1));

-

-                                                if (colObjA.IslandTag == islandId)

-                                                {

-                                                        colObjA.ActivationState =ActivationState.IslandSleeping;

-                                                }

-                                        }

-                                }

-                                else

-                                {

-                                        for (idx = startIslandIndex; idx < endIslandIndex; idx++)

-                                        {

-                                                int i = UnionFind[idx].Size;

-

-                                                CollisionObject colObjA = collisionObjects[i];

-                                                if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1))

-                                                {

-                                                        Console.WriteLine("error in island management");

-                                                }

-

-                                                BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1));

-

-                                                if (colObjA.IslandTag == islandId)

-                                                {

-                                                        if (colObjA.ActivationState == ActivationState.IslandSleeping)

-                                                        {

-                                                                colObjA.ActivationState = ActivationState.WantsDeactivation;

-                                                        }

-                                                }

-                                        }

-                                }

-                        }

-

-                        //int maxNumManifolds = dispatcher.ManifoldCount;

-                        List<PersistentManifold> islandmanifold = new List<PersistentManifold>(dispatcher.ManifoldCount);

-

-                        for (int i = 0; i < dispatcher.ManifoldCount; i++)

-                        {

-                                PersistentManifold manifold = dispatcher.GetManifoldByIndex(i);

-

-                                CollisionObject colObjA = manifold.BodyA as CollisionObject;

-                                CollisionObject colObjB = manifold.BodyB as CollisionObject;

-

-                                //todo: check sleeping conditions!

-                                if (((colObjA != null) && colObjA.ActivationState != ActivationState.IslandSleeping) ||

-                                   ((colObjB != null) && colObjB.ActivationState != ActivationState.IslandSleeping))

-                                {

-

-                                        //kinematic objects don't merge islands, but wake up all connected objects

-                                        if (colObjA.IsStaticOrKinematicObject && colObjA.ActivationState != ActivationState.IslandSleeping)

-                                        {

-                                                colObjB.Activate();

-                                        }

-                                        if (colObjB.IsStaticOrKinematicObject && colObjB.ActivationState != ActivationState.IslandSleeping)

-                                        {

-                                                colObjA.Activate();

-                                        }

-

-                                        //filtering for response

-                                        if (dispatcher.NeedsResponse(colObjA, colObjB))

-                                                islandmanifold.Add(manifold);

-                                }

-                        }

-

-                        int numManifolds = islandmanifold.Count;

-

-                        // Sort manifolds, based on islands

-                        // Sort the vector using predicate and std::sort

-                        islandmanifold.Sort(new Comparison<PersistentManifold>(PersistentManifoldSortPredicate));

-

-                        //now process all active islands (sets of manifolds for now)

-                        int startManifoldIndex = 0;

-                        int endManifoldIndex = 1;

-

-                        List<CollisionObject> islandBodies = new List<CollisionObject>();

-

-                        for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)

-                        {

-                                int islandId = UnionFind[startIslandIndex].ID;

-                                bool islandSleeping = false;

-                                for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++)

-                                {

-                                        int i = UnionFind[endIslandIndex].Size;

-                                        CollisionObject colObjA = collisionObjects[i];

-                                        islandBodies.Add(colObjA);

-                                        if (!colObjA.IsActive)

-                                                islandSleeping = true;

-                                }

-

-                                //find the accompanying contact manifold for this islandId

-                                int numIslandManifolds = 0;

-                                List<PersistentManifold> startManifold = new List<PersistentManifold>(numIslandManifolds);

-

-                                if (startManifoldIndex < numManifolds)

-                                {

-                                        int curIslandID = GetIslandId(islandmanifold[startManifoldIndex]);

-                                        if (curIslandID == islandId)

-                                        {

-                                                for (int k = startManifoldIndex; k < islandmanifold.Count; k++)

-                                                {

-                                                        startManifold.Add(islandmanifold[k]);

-                                                }

-                                                for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == GetIslandId(islandmanifold[endManifoldIndex])); endManifoldIndex++) { }

-

-                                                // Process the actual simulation, only if not sleeping/deactivated

-                                                numIslandManifolds = endManifoldIndex - startManifoldIndex;

-                                        }

-                                }

-

-                                if (!islandSleeping)

-                                {

-                                        callback.ProcessIsland(islandBodies, startManifold, numIslandManifolds, islandId);

-                                }

-

-                                if (numIslandManifolds != 0)

-                                {

-                                        startManifoldIndex = endManifoldIndex;

-                                }

-

-                                islandBodies.Clear();

-                        }

-                }

-

-                private static int GetIslandId(PersistentManifold lhs)

-                {

-                        int islandId;

-                        CollisionObject rcolObjA = lhs.BodyA as CollisionObject;

-                        CollisionObject rcolObjB = lhs.BodyB as CollisionObject;

-                        islandId = rcolObjA.IslandTag >= 0 ? rcolObjA.IslandTag : rcolObjB.IslandTag;

-                        return islandId;

-                }

-

-                private static int PersistentManifoldSortPredicate(PersistentManifold lhs, PersistentManifold rhs)

-                {

-                        int rIslandIdA, lIslandIdB;

-                        rIslandIdA = GetIslandId(rhs);

-                        lIslandIdB = GetIslandId(lhs);

-                        //return lIslandId0 < rIslandId0;

-                        if (lIslandIdB < rIslandIdA)

-                                return -1;

-                        //else if (lIslandIdB > rIslandIdA)

-                        //    return 1;

-                        return 1;

-                }

-

-                public interface IIslandCallback

-                {

-                        void ProcessIsland(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, int islandID);

-                }

-        }

-}

+/*
+  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 SimulationIslandManager
+        {
+                private UnionFind _unionFind = new UnionFind();
+
+                public void InitUnionFind(int n)
+                {
+                        _unionFind.Reset(n);
+                }
+
+                public UnionFind UnionFind { get { return _unionFind; } }
+
+                public virtual void UpdateActivationState(CollisionWorld world, IDispatcher dispatcher)
+                {
+                        InitUnionFind(world.CollisionObjectsCount);
+
+                        // put the index into m_controllers into m_tag  
+                        int index = 0;
+                        for (int i = 0; i < world.CollisionObjects.Count; i++)
+                        {
+                                world.CollisionObjects[i].IslandTag = index;
+                                world.CollisionObjects[i].HitFraction = 1;
+                                world.CollisionObjects[i].CompanionID = -1;
+                                index++;
+                        }
+                        // do the union find
+                        FindUnions(dispatcher);
+                }
+
+                public virtual void StoreIslandActivationState(CollisionWorld world)
+                {
+                        // put the islandId ('find' value) into m_tag                   
+                        int index = 0;
+                        for (int i = 0; i < world.CollisionObjects.Count; i++)
+                        {
+                                if (world.CollisionObjects[i].MergesSimulationIslands)
+                                {
+                                        world.CollisionObjects[i].IslandTag = _unionFind.Find(index);
+                                        world.CollisionObjects[i].CompanionID = -1;
+                                }
+                                else
+                                {
+                                        world.CollisionObjects[i].IslandTag = -1;
+                                        world.CollisionObjects[i].CompanionID = -2;
+                                }
+                                index++;
+                        }
+                }
+
+                public void FindUnions(IDispatcher dispatcher)
+                {
+                        for (int i = 0; i < dispatcher.ManifoldCount; i++)
+                        {
+                                PersistentManifold manifold = dispatcher.GetManifoldByIndex(i);
+                                //static objects (invmass 0.f) don't merge !
+
+                                CollisionObject colObjA = manifold.BodyA as CollisionObject;
+                                CollisionObject colObjB = manifold.BodyB as CollisionObject;
+
+                                if (((colObjA != null) && (colObjA.MergesSimulationIslands)) &&
+                                        ((colObjB != null) && (colObjB.MergesSimulationIslands)))
+                                {
+                                        _unionFind.Unite(colObjA.IslandTag, colObjB.IslandTag);
+                                }
+                        }
+                }
+
+                public void BuildAndProcessIslands(IDispatcher dispatcher, List<CollisionObject> collisionObjects, IIslandCallback callback)
+                {
+                        //we are going to sort the unionfind array, and store the element id in the size
+                        //afterwards, we clean unionfind, to make sure no-one uses it anymore
+                        UnionFind.SortIslands();
+                        int numElem = UnionFind.ElementCount;
+
+                        int endIslandIndex = 1;
+                        int startIslandIndex;
+
+                        //update the sleeping state for bodies, if all are sleeping
+                        for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
+                        {
+                                int islandId = UnionFind[startIslandIndex].ID;
+                                for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++)
+                                {
+                                }
+
+                                //int numSleeping = 0;
+
+                                bool allSleeping = true;
+
+                                int idx;
+                                for (idx = startIslandIndex; idx < endIslandIndex; idx++)
+                                {
+                                        int i = UnionFind[idx].Size;
+
+                                        CollisionObject colObjA = collisionObjects[i];
+                                        if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1))
+                                        {
+                                                Console.WriteLine("error in island management");
+                                        }
+
+                                        BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1));
+                                        if (colObjA.IslandTag == islandId)
+                                        {
+                                                if (colObjA.ActivationState == ActivationState.Active)
+                                                {
+                                                        allSleeping = false;
+                                                }
+                                                if (colObjA.ActivationState == ActivationState.DisableDeactivation)
+                                                {
+                                                        allSleeping = false;
+                                                }
+                                        }
+                                }
+
+
+                                if (allSleeping)
+                                {
+                                        for (idx = startIslandIndex; idx < endIslandIndex; idx++)
+                                        {
+                                                int i = UnionFind[idx].Size;
+                                                CollisionObject colObjA = collisionObjects[i];
+                                                if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1))
+                                                {
+                                                        Console.WriteLine("error in island management");
+                                                }
+
+                                                BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1));
+
+                                                if (colObjA.IslandTag == islandId)
+                                                {
+                                                        colObjA.ActivationState =ActivationState.IslandSleeping;
+                                                }
+                                        }
+                                }
+                                else
+                                {
+                                        for (idx = startIslandIndex; idx < endIslandIndex; idx++)
+                                        {
+                                                int i = UnionFind[idx].Size;
+
+                                                CollisionObject colObjA = collisionObjects[i];
+                                                if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1))
+                                                {
+                                                        Console.WriteLine("error in island management");
+                                                }
+
+                                                BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1));
+
+                                                if (colObjA.IslandTag == islandId)
+                                                {
+                                                        if (colObjA.ActivationState == ActivationState.IslandSleeping)
+                                                        {
+                                                                colObjA.ActivationState = ActivationState.WantsDeactivation;
+                                                        }
+                                                }
+                                        }
+                                }
+                        }
+
+                        //int maxNumManifolds = dispatcher.ManifoldCount;
+                        List<PersistentManifold> islandmanifold = new List<PersistentManifold>(dispatcher.ManifoldCount);
+
+                        for (int i = 0; i < dispatcher.ManifoldCount; i++)
+                        {
+                                PersistentManifold manifold = dispatcher.GetManifoldByIndex(i);
+
+                                CollisionObject colObjA = manifold.BodyA as CollisionObject;
+                                CollisionObject colObjB = manifold.BodyB as CollisionObject;
+
+                                //todo: check sleeping conditions!
+                                if (((colObjA != null) && colObjA.ActivationState != ActivationState.IslandSleeping) ||
+                                   ((colObjB != null) && colObjB.ActivationState != ActivationState.IslandSleeping))
+                                {
+
+                                        //kinematic objects don't merge islands, but wake up all connected objects
+                                        if (colObjA.IsStaticOrKinematicObject && colObjA.ActivationState != ActivationState.IslandSleeping)
+                                        {
+                                                colObjB.Activate();
+                                        }
+                                        if (colObjB.IsStaticOrKinematicObject && colObjB.ActivationState != ActivationState.IslandSleeping)
+                                        {
+                                                colObjA.Activate();
+                                        }
+
+                                        //filtering for response
+                                        if (dispatcher.NeedsResponse(colObjA, colObjB))
+                                                islandmanifold.Add(manifold);
+                                }
+                        }
+
+                        int numManifolds = islandmanifold.Count;
+
+                        // Sort manifolds, based on islands
+                        // Sort the vector using predicate and std::sort
+                        islandmanifold.Sort(new Comparison<PersistentManifold>(PersistentManifoldSortPredicate));
+
+                        //now process all active islands (sets of manifolds for now)
+                        int startManifoldIndex = 0;
+                        int endManifoldIndex = 1;
+
+                        List<CollisionObject> islandBodies = new List<CollisionObject>();
+
+                        for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
+                        {
+                                int islandId = UnionFind[startIslandIndex].ID;
+                                bool islandSleeping = false;
+                                for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++)
+                                {
+                                        int i = UnionFind[endIslandIndex].Size;
+                                        CollisionObject colObjA = collisionObjects[i];
+                                        islandBodies.Add(colObjA);
+                                        if (!colObjA.IsActive)
+                                                islandSleeping = true;
+                                }
+
+                                //find the accompanying contact manifold for this islandId
+                                int numIslandManifolds = 0;
+                                List<PersistentManifold> startManifold = new List<PersistentManifold>(numIslandManifolds);
+
+                                if (startManifoldIndex < numManifolds)
+                                {
+                                        int curIslandID = GetIslandId(islandmanifold[startManifoldIndex]);
+                                        if (curIslandID == islandId)
+                                        {
+                                                for (int k = startManifoldIndex; k < islandmanifold.Count; k++)
+                                                {
+                                                        startManifold.Add(islandmanifold[k]);
+                                                }
+                                                for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == GetIslandId(islandmanifold[endManifoldIndex])); endManifoldIndex++) { }
+
+                                                // Process the actual simulation, only if not sleeping/deactivated
+                                                numIslandManifolds = endManifoldIndex - startManifoldIndex;
+                                        }
+                                }
+
+                                if (!islandSleeping)
+                                {
+                                        callback.ProcessIsland(islandBodies, startManifold, numIslandManifolds, islandId);
+                                }
+
+                                if (numIslandManifolds != 0)
+                                {
+                                        startManifoldIndex = endManifoldIndex;
+                                }
+
+                                islandBodies.Clear();
+                        }
+                }
+
+                private static int GetIslandId(PersistentManifold lhs)
+                {
+                        int islandId;
+                        CollisionObject rcolObjA = lhs.BodyA as CollisionObject;
+                        CollisionObject rcolObjB = lhs.BodyB as CollisionObject;
+                        islandId = rcolObjA.IslandTag >= 0 ? rcolObjA.IslandTag : rcolObjB.IslandTag;
+                        return islandId;
+                }
+
+                private static int PersistentManifoldSortPredicate(PersistentManifold lhs, PersistentManifold rhs)
+                {
+                        int rIslandIdA, lIslandIdB;
+                        rIslandIdA = GetIslandId(rhs);
+                        lIslandIdB = GetIslandId(lhs);
+                        //return lIslandId0 < rIslandId0;
+                        if (lIslandIdB < rIslandIdA)
+                                return -1;
+                        //else if (lIslandIdB > rIslandIdA)
+                        //    return 1;
+                        return 1;
+                }
+
+                public interface IIslandCallback
+                {
+                        void ProcessIsland(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, int islandID);
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs
index 8933638..2b4e801 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs
@@ -1,270 +1,270 @@
-/*

-  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>

-        /// SphereBoxCollisionAlgorithm  provides sphere-box collision detection.

-        /// Other features are frame-coherency (persistent data) and collision response.

-        /// </summary>

-        public class SphereBoxCollisionAlgorithm : CollisionAlgorithm, IDisposable

-        {

-                private bool _ownManifold;

-                private PersistentManifold _manifold;

-                private bool _isSwapped;

-

-                public SphereBoxCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject collisionObjectA, CollisionObject collisionObjectB, bool isSwapped)

-                        : base(collisionAlgorithmConstructionInfo)

-                {

-                        _ownManifold = false;

-                        _manifold = manifold;

-                        _isSwapped = isSwapped;

-

-                        CollisionObject sphereObject = _isSwapped ? collisionObjectB : collisionObjectA;

-                        CollisionObject boxObject = _isSwapped ? collisionObjectA : collisionObjectB;

-

-                        if (_manifold == null && Dispatcher.NeedsCollision(sphereObject, boxObject))

-                        {

-                                _manifold = Dispatcher.GetNewManifold(sphereObject, boxObject);

-                                _ownManifold = true;

-                        }

-                }

-

-                public float GetSphereDistance(CollisionObject boxObject, out Vector3 pointOnBox, out Vector3 pointOnSphere, Vector3 sphereCenter, float radius)

-                {

-                        pointOnBox = new Vector3();

-                        pointOnSphere = new Vector3();

-

-                        float margins;

-                        Vector3[] bounds = new Vector3[2];

-                        BoxShape boxShape = boxObject.CollisionShape as BoxShape;

-

-                        bounds[0] = -boxShape.HalfExtents;

-                        bounds[1] = boxShape.HalfExtents;

-

-                        margins = boxShape.Margin; //also add sphereShape margin?

-

-                        Matrix m44T = boxObject.WorldTransform;

-

-                        Vector3[] boundsVec = new Vector3[2];

-                        float penetration;

-

-                        boundsVec[0] = bounds[0];

-                        boundsVec[1] = bounds[1];

-

-                        Vector3 marginsVec = new Vector3(margins, margins, margins);

-

-                        // add margins

-                        bounds[0] += marginsVec;

-                        bounds[1] -= marginsVec;

-

-                        /////////////////////////////////////////////////

-

-                        Vector3 tmp, prel, normal, v3P;

-                        Vector3[] n = new Vector3[6];

-                        float sep = 10000000.0f, sepThis;

-

-                        n[0] = new Vector3(-1.0f, 0.0f, 0.0f);

-                        n[1] = new Vector3(0.0f, -1.0f, 0.0f);

-                        n[2] = new Vector3(0.0f, 0.0f, -1.0f);

-                        n[3] = new Vector3(1.0f, 0.0f, 0.0f);

-                        n[4] = new Vector3(0.0f, 1.0f, 0.0f);

-                        n[5] = new Vector3(0.0f, 0.0f, 1.0f);

-

-                        // convert  point in local space

-                        prel = MathHelper.InvXForm(m44T, sphereCenter);

-

-                        bool found = false;

-

-                        v3P = prel;

-

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

-                        {

-                                int j = i < 3 ? 0 : 1;

-                                if ((sepThis = (Vector3.Dot(v3P - bounds[j], n[i]))) > 0.0f)

-                                {

-                                        v3P = v3P - n[i] * sepThis;

-                                        found = true;

-                                }

-                        }

-

-                        //

-

-                        if (found)

-                        {

-                                bounds[0] = boundsVec[0];

-                                bounds[1] = boundsVec[1];

-

-                                normal = Vector3.Normalize(prel - v3P);

-                                pointOnBox = v3P + normal * margins;

-                                pointOnSphere = prel - normal * radius;

-

-                                if ((Vector3.Dot(pointOnSphere - pointOnBox, normal)) > 0.0f)

-                                {

-                                        return 1.0f;

-                                }

-

-                                // transform back in world space

-                                tmp = MathHelper.MatrixToVector(m44T, pointOnBox);

-                                pointOnBox = tmp;

-                                tmp = MathHelper.MatrixToVector(m44T, pointOnSphere);

-                                pointOnSphere = tmp;

-                                float seps2 = (pointOnBox - pointOnSphere).LengthSquared();

-

-                                //if this fails, fallback into deeper penetration case, below

-                                if (seps2 > MathHelper.Epsilon)

-                                {

-                                        sep = -(float)Math.Sqrt(seps2);

-                                        normal = (pointOnBox - pointOnSphere);

-                                        normal *= 1f / sep;

-                                }

-                                return sep;

-                        }

-

-                        //////////////////////////////////////////////////

-                        // Deep penetration case

-

-                        penetration = GetSpherePenetration(boxObject, ref pointOnBox, ref pointOnSphere, sphereCenter, radius, bounds[0], bounds[1]);

-

-                        bounds[0] = boundsVec[0];

-                        bounds[1] = boundsVec[1];

-

-                        if (penetration <= 0.0f)

-                                return (penetration - margins);

-                        else

-                                return 1.0f;

-                }

-

-                public float GetSpherePenetration(CollisionObject boxObject, ref Vector3 pointOnBox, ref Vector3 pointOnSphere, Vector3 sphereCenter, float radius, Vector3 aabbMin, Vector3 aabbMax)

-                {

-                        Vector3[] bounds = new Vector3[2];

-

-                        bounds[0] = aabbMin;

-                        bounds[1] = aabbMax;

-

-                        Vector3 p0 = new Vector3(), tmp, prel, normal = new Vector3();

-                        Vector3[] n = new Vector3[6];

-                        float sep = -10000000.0f, sepThis;

-

-                        n[0] = new Vector3(-1.0f, 0.0f, 0.0f);

-                        n[1] = new Vector3(0.0f, -1.0f, 0.0f);

-                        n[2] = new Vector3(0.0f, 0.0f, -1.0f);

-                        n[3] = new Vector3(1.0f, 0.0f, 0.0f);

-                        n[4] = new Vector3(0.0f, 1.0f, 0.0f);

-                        n[5] = new Vector3(0.0f, 0.0f, 1.0f);

-

-                        Matrix m44T = boxObject.WorldTransform;

-

-                        // convert point in local space

-                        prel = MathHelper.InvXForm(m44T, sphereCenter);

-

-                        ///////////

-

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

-                        {

-                                int j = i < 3 ? 0 : 1;

-                                if ((sepThis = (Vector3.Dot(prel - bounds[j], n[i])) - radius) > 0.0f) return 1.0f;

-                                if (sepThis > sep)

-                                {

-                                        p0 = bounds[j];

-                                        normal = n[i];

-                                        sep = sepThis;

-                                }

-                        }

-

-                        pointOnBox = prel - normal * (Vector3.Dot(normal, (prel - p0)));

-                        pointOnSphere = pointOnBox + normal * sep;

-

-                        // transform back in world space

-                        tmp = MathHelper.MatrixToVector(m44T, pointOnBox);

-                        pointOnBox = tmp;

-                        tmp = MathHelper.MatrixToVector(m44T, pointOnSphere);

-                        pointOnSphere = tmp;

-                        normal = Vector3.Normalize(pointOnBox - pointOnSphere);

-

-                        return sep;

-                }

-

-                public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)

-                {

-                        if (_manifold == null)

-                                return;

-

-                        CollisionObject sphereObject = _isSwapped ? bodyB : bodyA;

-                        CollisionObject boxObject = _isSwapped ? bodyA : bodyB;

-

-                        SphereShape sphereA = sphereObject.CollisionShape as SphereShape;

-

-                        Vector3 pOnBox, pOnSphere;

-                        Vector3 sphereCenter = sphereObject.WorldTransform.Translation;

-                        float radius = sphereA.Radius;

-

-                        float dist = GetSphereDistance(boxObject, out pOnBox, out pOnSphere, sphereCenter, radius);

-

-                        if (dist < MathHelper.Epsilon)

-                        {

-                                Vector3 normalOnSurfaceB = Vector3.Normalize(pOnBox - pOnSphere);

-

-                                // report a contact. internally this will be kept persistent, and contact reduction is done

-                                resultOut.SetPersistentManifold(_manifold);

-                                resultOut.AddContactPoint(normalOnSurfaceB, pOnBox, dist);

-                        }

-                }

-

-                public override float CalculateTimeOfImpact(CollisionObject collisionObjectA, CollisionObject collisionObjectB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)

-                {

-                        //not yet

-                        return 1;

-                }

-

-                public class CreateFunc : CollisionAlgorithmCreateFunction

-                {

-                        public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)

-                        {

-                                if (!IsSwapped)

-                                        return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, false);

-                                else

-                                        return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, true);

-                        }

-                }

-

-                #region IDisposable Members

-                public void Dispose()

-                {

-                        Dispose(true);

-                }

-

-                public void Dispose(bool disposing)

-                {

-                        if (disposing && _ownManifold)

-                        {

-                                if (_manifold != null)

-                                        Dispatcher.ReleaseManifold(_manifold);

-                        }

-                }

-                #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>
+        /// SphereBoxCollisionAlgorithm  provides sphere-box collision detection.
+        /// Other features are frame-coherency (persistent data) and collision response.
+        /// </summary>
+        public class SphereBoxCollisionAlgorithm : CollisionAlgorithm, IDisposable
+        {
+                private bool _ownManifold;
+                private PersistentManifold _manifold;
+                private bool _isSwapped;
+
+                public SphereBoxCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject collisionObjectA, CollisionObject collisionObjectB, bool isSwapped)
+                        : base(collisionAlgorithmConstructionInfo)
+                {
+                        _ownManifold = false;
+                        _manifold = manifold;
+                        _isSwapped = isSwapped;
+
+                        CollisionObject sphereObject = _isSwapped ? collisionObjectB : collisionObjectA;
+                        CollisionObject boxObject = _isSwapped ? collisionObjectA : collisionObjectB;
+
+                        if (_manifold == null && Dispatcher.NeedsCollision(sphereObject, boxObject))
+                        {
+                                _manifold = Dispatcher.GetNewManifold(sphereObject, boxObject);
+                                _ownManifold = true;
+                        }
+                }
+
+                public float GetSphereDistance(CollisionObject boxObject, out Vector3 pointOnBox, out Vector3 pointOnSphere, Vector3 sphereCenter, float radius)
+                {
+                        pointOnBox = new Vector3();
+                        pointOnSphere = new Vector3();
+
+                        float margins;
+                        Vector3[] bounds = new Vector3[2];
+                        BoxShape boxShape = boxObject.CollisionShape as BoxShape;
+
+                        bounds[0] = -boxShape.HalfExtents;
+                        bounds[1] = boxShape.HalfExtents;
+
+                        margins = boxShape.Margin; //also add sphereShape margin?
+
+                        Matrix m44T = boxObject.WorldTransform;
+
+                        Vector3[] boundsVec = new Vector3[2];
+                        float penetration;
+
+                        boundsVec[0] = bounds[0];
+                        boundsVec[1] = bounds[1];
+
+                        Vector3 marginsVec = new Vector3(margins, margins, margins);
+
+                        // add margins
+                        bounds[0] += marginsVec;
+                        bounds[1] -= marginsVec;
+
+                        /////////////////////////////////////////////////
+
+                        Vector3 tmp, prel, normal, v3P;
+                        Vector3[] n = new Vector3[6];
+                        float sep = 10000000.0f, sepThis;
+
+                        n[0] = new Vector3(-1.0f, 0.0f, 0.0f);
+                        n[1] = new Vector3(0.0f, -1.0f, 0.0f);
+                        n[2] = new Vector3(0.0f, 0.0f, -1.0f);
+                        n[3] = new Vector3(1.0f, 0.0f, 0.0f);
+                        n[4] = new Vector3(0.0f, 1.0f, 0.0f);
+                        n[5] = new Vector3(0.0f, 0.0f, 1.0f);
+
+                        // convert  point in local space
+                        prel = MathHelper.InvXForm(m44T, sphereCenter);
+
+                        bool found = false;
+
+                        v3P = prel;
+
+                        for (int i = 0; i < 6; i++)
+                        {
+                                int j = i < 3 ? 0 : 1;
+                                if ((sepThis = (Vector3.Dot(v3P - bounds[j], n[i]))) > 0.0f)
+                                {
+                                        v3P = v3P - n[i] * sepThis;
+                                        found = true;
+                                }
+                        }
+
+                        //
+
+                        if (found)
+                        {
+                                bounds[0] = boundsVec[0];
+                                bounds[1] = boundsVec[1];
+
+                                normal = Vector3.Normalize(prel - v3P);
+                                pointOnBox = v3P + normal * margins;
+                                pointOnSphere = prel - normal * radius;
+
+                                if ((Vector3.Dot(pointOnSphere - pointOnBox, normal)) > 0.0f)
+                                {
+                                        return 1.0f;
+                                }
+
+                                // transform back in world space
+                                tmp = MathHelper.MatrixToVector(m44T, pointOnBox);
+                                pointOnBox = tmp;
+                                tmp = MathHelper.MatrixToVector(m44T, pointOnSphere);
+                                pointOnSphere = tmp;
+                                float seps2 = (pointOnBox - pointOnSphere).LengthSquared();
+
+                                //if this fails, fallback into deeper penetration case, below
+                                if (seps2 > MathHelper.Epsilon)
+                                {
+                                        sep = -(float)Math.Sqrt(seps2);
+                                        normal = (pointOnBox - pointOnSphere);
+                                        normal *= 1f / sep;
+                                }
+                                return sep;
+                        }
+
+                        //////////////////////////////////////////////////
+                        // Deep penetration case
+
+                        penetration = GetSpherePenetration(boxObject, ref pointOnBox, ref pointOnSphere, sphereCenter, radius, bounds[0], bounds[1]);
+
+                        bounds[0] = boundsVec[0];
+                        bounds[1] = boundsVec[1];
+
+                        if (penetration <= 0.0f)
+                                return (penetration - margins);
+                        else
+                                return 1.0f;
+                }
+
+                public float GetSpherePenetration(CollisionObject boxObject, ref Vector3 pointOnBox, ref Vector3 pointOnSphere, Vector3 sphereCenter, float radius, Vector3 aabbMin, Vector3 aabbMax)
+                {
+                        Vector3[] bounds = new Vector3[2];
+
+                        bounds[0] = aabbMin;
+                        bounds[1] = aabbMax;
+
+                        Vector3 p0 = new Vector3(), tmp, prel, normal = new Vector3();
+                        Vector3[] n = new Vector3[6];
+                        float sep = -10000000.0f, sepThis;
+
+                        n[0] = new Vector3(-1.0f, 0.0f, 0.0f);
+                        n[1] = new Vector3(0.0f, -1.0f, 0.0f);
+                        n[2] = new Vector3(0.0f, 0.0f, -1.0f);
+                        n[3] = new Vector3(1.0f, 0.0f, 0.0f);
+                        n[4] = new Vector3(0.0f, 1.0f, 0.0f);
+                        n[5] = new Vector3(0.0f, 0.0f, 1.0f);
+
+                        Matrix m44T = boxObject.WorldTransform;
+
+                        // convert point in local space
+                        prel = MathHelper.InvXForm(m44T, sphereCenter);
+
+                        ///////////
+
+                        for (int i = 0; i < 6; i++)
+                        {
+                                int j = i < 3 ? 0 : 1;
+                                if ((sepThis = (Vector3.Dot(prel - bounds[j], n[i])) - radius) > 0.0f) return 1.0f;
+                                if (sepThis > sep)
+                                {
+                                        p0 = bounds[j];
+                                        normal = n[i];
+                                        sep = sepThis;
+                                }
+                        }
+
+                        pointOnBox = prel - normal * (Vector3.Dot(normal, (prel - p0)));
+                        pointOnSphere = pointOnBox + normal * sep;
+
+                        // transform back in world space
+                        tmp = MathHelper.MatrixToVector(m44T, pointOnBox);
+                        pointOnBox = tmp;
+                        tmp = MathHelper.MatrixToVector(m44T, pointOnSphere);
+                        pointOnSphere = tmp;
+                        normal = Vector3.Normalize(pointOnBox - pointOnSphere);
+
+                        return sep;
+                }
+
+                public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
+                {
+                        if (_manifold == null)
+                                return;
+
+                        CollisionObject sphereObject = _isSwapped ? bodyB : bodyA;
+                        CollisionObject boxObject = _isSwapped ? bodyA : bodyB;
+
+                        SphereShape sphereA = sphereObject.CollisionShape as SphereShape;
+
+                        Vector3 pOnBox, pOnSphere;
+                        Vector3 sphereCenter = sphereObject.WorldTransform.Translation;
+                        float radius = sphereA.Radius;
+
+                        float dist = GetSphereDistance(boxObject, out pOnBox, out pOnSphere, sphereCenter, radius);
+
+                        if (dist < MathHelper.Epsilon)
+                        {
+                                Vector3 normalOnSurfaceB = Vector3.Normalize(pOnBox - pOnSphere);
+
+                                // report a contact. internally this will be kept persistent, and contact reduction is done
+                                resultOut.SetPersistentManifold(_manifold);
+                                resultOut.AddContactPoint(normalOnSurfaceB, pOnBox, dist);
+                        }
+                }
+
+                public override float CalculateTimeOfImpact(CollisionObject collisionObjectA, CollisionObject collisionObjectB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
+                {
+                        //not yet
+                        return 1;
+                }
+
+                public class CreateFunc : CollisionAlgorithmCreateFunction
+                {
+                        public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
+                        {
+                                if (!IsSwapped)
+                                        return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, false);
+                                else
+                                        return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, true);
+                        }
+                }
+
+                #region IDisposable Members
+                public void Dispose()
+                {
+                        Dispose(true);
+                }
+
+                public void Dispose(bool disposing)
+                {
+                        if (disposing && _ownManifold)
+                        {
+                                if (_manifold != null)
+                                        Dispatcher.ReleaseManifold(_manifold);
+                        }
+                }
+                #endregion
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs
index 7a76d24..3a780c1 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs
@@ -1,104 +1,104 @@
-/*

-  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 SphereSphereCollisionAlgorithm : CollisionAlgorithm

-        {

-                private bool _ownManifold;

-                private PersistentManifold _manifold;

-

-                public SphereSphereCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)

-                        : base(collisionAlgorithmConstructionInfo)

-                {

-                        _ownManifold = false;

-                        _manifold = manifold;

-

-                        if (_manifold == null)

-                        {

-                                _manifold = Dispatcher.GetNewManifold(bodyA, bodyB);

-                                _ownManifold = true;

-                        }

-                }

-

-                public SphereSphereCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo)

-                        : base(collisionAlgorithmConstructionInfo) { }

-

-                ~SphereSphereCollisionAlgorithm()

-                {

-                        if (_ownManifold)

-                        {

-                                if (_manifold != null)

-                                        Dispatcher.ReleaseManifold(_manifold);

-                        }

-                }

-

-                public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)

-                {

-                        if (_manifold == null)

-                                return;

-

-                        SphereShape sphereA = bodyA.CollisionShape as SphereShape;

-                        SphereShape sphereB = bodyB.CollisionShape as SphereShape;

-

-                        Vector3 diff = bodyA.WorldTransform.Translation - bodyB.WorldTransform.Translation;

-                        float len = diff.Length();

-                        float radiusA = sphereA.Radius;

-                        float radiusB = sphereB.Radius;

-

-                        //if distance positive, don't generate a new contact

-                        if (len > (radiusA + radiusB))

-                                return;

-

-                        //distance (negative means penetration)

-                        float dist = len - (radiusA + radiusB);

-

-                        Vector3 normalOnSurfaceB = diff / len;

-                        //point on A (worldspace)

-                        Vector3 posA = bodyA.WorldTransform.Translation - radiusA * normalOnSurfaceB;

-                        //point on B (worldspace)

-                        Vector3 posB = bodyB.WorldTransform.Translation + radiusB * normalOnSurfaceB;

-

-                        // report a contact. internally this will be kept persistent, and contact reduction is done

-                        resultOut.SetPersistentManifold(_manifold);

-                        resultOut.AddContactPoint(normalOnSurfaceB, posB, dist);

-                }

-

-                public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)

-                {

-                        //not yet

-                        return 1f;

-                }

-

-                public class CreateFunc : CollisionAlgorithmCreateFunction

-                {

-                        public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)

-                        {

-                                return new SphereSphereCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB);

-                        }

-                }

-        }

-}

+/*
+  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 SphereSphereCollisionAlgorithm : CollisionAlgorithm
+        {
+                private bool _ownManifold;
+                private PersistentManifold _manifold;
+
+                public SphereSphereCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
+                        : base(collisionAlgorithmConstructionInfo)
+                {
+                        _ownManifold = false;
+                        _manifold = manifold;
+
+                        if (_manifold == null)
+                        {
+                                _manifold = Dispatcher.GetNewManifold(bodyA, bodyB);
+                                _ownManifold = true;
+                        }
+                }
+
+                public SphereSphereCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo)
+                        : base(collisionAlgorithmConstructionInfo) { }
+
+                ~SphereSphereCollisionAlgorithm()
+                {
+                        if (_ownManifold)
+                        {
+                                if (_manifold != null)
+                                        Dispatcher.ReleaseManifold(_manifold);
+                        }
+                }
+
+                public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
+                {
+                        if (_manifold == null)
+                                return;
+
+                        SphereShape sphereA = bodyA.CollisionShape as SphereShape;
+                        SphereShape sphereB = bodyB.CollisionShape as SphereShape;
+
+                        Vector3 diff = bodyA.WorldTransform.Translation - bodyB.WorldTransform.Translation;
+                        float len = diff.Length();
+                        float radiusA = sphereA.Radius;
+                        float radiusB = sphereB.Radius;
+
+                        //if distance positive, don't generate a new contact
+                        if (len > (radiusA + radiusB))
+                                return;
+
+                        //distance (negative means penetration)
+                        float dist = len - (radiusA + radiusB);
+
+                        Vector3 normalOnSurfaceB = diff / len;
+                        //point on A (worldspace)
+                        Vector3 posA = bodyA.WorldTransform.Translation - radiusA * normalOnSurfaceB;
+                        //point on B (worldspace)
+                        Vector3 posB = bodyB.WorldTransform.Translation + radiusB * normalOnSurfaceB;
+
+                        // report a contact. internally this will be kept persistent, and contact reduction is done
+                        resultOut.SetPersistentManifold(_manifold);
+                        resultOut.AddContactPoint(normalOnSurfaceB, posB, dist);
+                }
+
+                public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
+                {
+                        //not yet
+                        return 1f;
+                }
+
+                public class CreateFunc : CollisionAlgorithmCreateFunction
+                {
+                        public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
+                        {
+                                return new SphereSphereCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB);
+                        }
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs
index 1ca5cfb..00dfaf1 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs
@@ -1,100 +1,100 @@
-/*

-  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>

-        /// SphereSphereCollisionAlgorithm  provides sphere-sphere collision detection.

-        /// Other features are frame-coherency (persistent data) and collision response.

-        /// Also provides the most basic sample for custom/user btCollisionAlgorithm

-        /// </summary>

-        public class SphereTriangleCollisionAlgorithm : CollisionAlgorithm, IDisposable

-        {

-                private bool _ownManifold;

-                private PersistentManifold _manifold;

-                private bool _isSwapped;

-

-                public SphereTriangleCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped)

-                        : base(collisionAlgorithmConstructionInfo)

-                {

-                        _ownManifold = false;

-                        _manifold = manifold;

-                        _isSwapped = isSwapped;

-

-                        if (_manifold == null)

-                        {

-                                _manifold = Dispatcher.GetNewManifold(bodyA, bodyB);

-                                _ownManifold = true;

-                        }

-                }

-

-                public SphereTriangleCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo)

-                        : base(collisionAlgorithmConstructionInfo) { }

-

-                public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)

-                {

-                        if (_manifold == null)

-                                return;

-

-                        SphereShape sphere = bodyA.CollisionShape as SphereShape;

-                        TriangleShape triangle = bodyB.CollisionShape as TriangleShape;

-

-                        /// report a contact. internally this will be kept persistent, and contact reduction is done

-                        resultOut.SetPersistentManifold(_manifold);

-                        SphereTriangleDetector detector = new SphereTriangleDetector(sphere, triangle);

-

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

-                        input.MaximumDistanceSquared = 1e30f;//todo: tighter bounds

-                        input.TransformA = bodyA.WorldTransform;

-                        input.TransformB = bodyB.WorldTransform;

-

-                        detector.GetClosestPoints(input, resultOut, null);

-                }

-

-                public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)

-                {

-                        //not yet

-                        return 1f;

-                }

-

-                public class CreateFunc : CollisionAlgorithmCreateFunction

-                {

-                        public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)

-                        {

-                                return new SphereTriangleCollisionAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, IsSwapped);

-                        }

-                }

-

-                #region IDisposable Members

-                public void Dispose()

-                {

-                        if (_ownManifold)

-                                if (_manifold != null)

-                                        Dispatcher.ReleaseManifold(_manifold);

-                }

-                #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>
+        /// SphereSphereCollisionAlgorithm  provides sphere-sphere collision detection.
+        /// Other features are frame-coherency (persistent data) and collision response.
+        /// Also provides the most basic sample for custom/user btCollisionAlgorithm
+        /// </summary>
+        public class SphereTriangleCollisionAlgorithm : CollisionAlgorithm, IDisposable
+        {
+                private bool _ownManifold;
+                private PersistentManifold _manifold;
+                private bool _isSwapped;
+
+                public SphereTriangleCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped)
+                        : base(collisionAlgorithmConstructionInfo)
+                {
+                        _ownManifold = false;
+                        _manifold = manifold;
+                        _isSwapped = isSwapped;
+
+                        if (_manifold == null)
+                        {
+                                _manifold = Dispatcher.GetNewManifold(bodyA, bodyB);
+                                _ownManifold = true;
+                        }
+                }
+
+                public SphereTriangleCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo)
+                        : base(collisionAlgorithmConstructionInfo) { }
+
+                public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
+                {
+                        if (_manifold == null)
+                                return;
+
+                        SphereShape sphere = bodyA.CollisionShape as SphereShape;
+                        TriangleShape triangle = bodyB.CollisionShape as TriangleShape;
+
+                        /// report a contact. internally this will be kept persistent, and contact reduction is done
+                        resultOut.SetPersistentManifold(_manifold);
+                        SphereTriangleDetector detector = new SphereTriangleDetector(sphere, triangle);
+
+                        DiscreteCollisionDetectorInterface.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
+                        input.MaximumDistanceSquared = 1e30f;//todo: tighter bounds
+                        input.TransformA = bodyA.WorldTransform;
+                        input.TransformB = bodyB.WorldTransform;
+
+                        detector.GetClosestPoints(input, resultOut, null);
+                }
+
+                public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
+                {
+                        //not yet
+                        return 1f;
+                }
+
+                public class CreateFunc : CollisionAlgorithmCreateFunction
+                {
+                        public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
+                        {
+                                return new SphereTriangleCollisionAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, IsSwapped);
+                        }
+                }
+
+                #region IDisposable Members
+                public void Dispose()
+                {
+                        if (_ownManifold)
+                                if (_manifold != null)
+                                        Dispatcher.ReleaseManifold(_manifold);
+                }
+                #endregion
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs
index 865754a..4d578d8 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs
@@ -1,214 +1,214 @@
-/*

-  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 SphereTriangleDetector : DiscreteCollisionDetectorInterface

-        {

-                private SphereShape _sphere;

-                private TriangleShape _triangle;

-                private const int MaxOverlap = 0;

-

-                public SphereTriangleDetector(SphereShape sphere, TriangleShape triangle)

-                {

-                        this._sphere = sphere;

-                        this._triangle = triangle;

-                }

-

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

-                {

-                        Matrix transformA = input.TransformA;

-                        Matrix transformB = input.TransformB;

-

-                        Vector3 point = new Vector3();

-                        Vector3 normal = new Vector3();

-                        Single timeOfImpact = 1.0f;

-                        Single depth = 0.0f;

-

-                        //move sphere into triangle space

-                        Matrix sphereInTr = MathHelper.InverseTimes(transformB, transformA);

-

-                        if (Collide(sphereInTr.Translation, point, normal, depth, timeOfImpact))

-                                output.AddContactPoint(Vector3.TransformNormal(normal, transformB), Vector3.TransformNormal(point, transformB), depth);

-                }

-

-                /// <summary>

-                /// See also geometrictools.com

-                /// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv

-                /// </summary>

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

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

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

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

-                /// <returns></returns>

-                private float SegmentSquareDistance(Vector3 from, Vector3 to, Vector3 point, Vector3 nearest)

-                {

-                        Vector3 diff = point - from;

-                        Vector3 v = to - from;

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

-

-                        if (t > 0)

-                        {

-                                float dotVV = Vector3.Dot(v, v);

-                                if (t < dotVV)

-                                {

-                                        t /= dotVV;

-                                        diff -= t * v;

-                                }

-                                else

-                                {

-                                        t = 1;

-                                        diff -= v;

-                                }

-                        }

-                        else

-                                t = 0;

-

-                        nearest = from + t * v;

-                        return Vector3.Dot(diff, diff);

-                }

-

-                private bool Collide(Vector3 sphereCenter, Vector3 point, Vector3 resultNormal, float depth, float timeOfImpact)

-                {

-                        Vector3[] vertices = _triangle.Vertices;

-                        Vector3 c = sphereCenter;

-                        float r = _sphere.Radius;

-

-                        Vector3 delta = new Vector3();

-

-                        Vector3 normal = Vector3.Cross(vertices[1] - vertices[0], vertices[2] - vertices[0]);

-                        normal = Vector3.Normalize(normal);

-                        Vector3 p1ToCentre = c - vertices[0];

-                        float distanceFromPlane = Vector3.Dot(p1ToCentre, normal);

-

-                        if (distanceFromPlane < 0)

-                        {

-                                //triangle facing the other way

-                                distanceFromPlane *= -1;

-                                normal *= -1;

-                        }

-

-                        float contactMargin = PersistentManifold.ContactBreakingThreshold;

-                        bool isInsideContactPlane = distanceFromPlane < r + contactMargin;

-                        bool isInsideShellPlane = distanceFromPlane < r;

-

-                        float deltaDotNormal = Vector3.Dot(delta, normal);

-                        if (!isInsideShellPlane && deltaDotNormal >= 0.0f)

-                                return false;

-

-                        // Check for contact / intersection

-                        bool hasContact = false;

-                        Vector3 contactPoint = new Vector3();

-                        if (isInsideContactPlane)

-                        {

-                                if (FaceContains(c, vertices, normal))

-                                {

-                                        // Inside the contact wedge - touches a point on the shell plane

-                                        hasContact = true;

-                                        contactPoint = c - normal * distanceFromPlane;

-                                }

-                                else

-                                {

-                                        // Could be inside one of the contact capsules

-                                        float contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin);

-                                        Vector3 nearestOnEdge = new Vector3();

-                                        for (int i = 0; i < _triangle.EdgeCount; i++)

-                                        {

-                                                Vector3 pa, pb;

-                                                _triangle.GetEdge(i, out pa, out pb);

-

-                                                float distanceSqr = SegmentSquareDistance(pa, pb, c, nearestOnEdge);

-                                                if (distanceSqr < contactCapsuleRadiusSqr)

-                                                {

-                                                        // Yep, we're inside a capsule

-                                                        hasContact = true;

-                                                        contactPoint = nearestOnEdge;

-                                                }

-                                        }

-                                }

-                        }

-

-                        if (hasContact)

-                        {

-                                Vector3 contactToCentre = c - contactPoint;

-                                float distanceSqr = contactToCentre.LengthSquared();

-                                if (distanceSqr < (r - MaxOverlap) * (r - MaxOverlap))

-                                {

-                                        float distance = (float)Math.Sqrt(distanceSqr);

-                                        resultNormal = contactToCentre;

-                                        resultNormal = Vector3.Normalize(resultNormal);

-                                        point = contactPoint;

-                                        depth = -(r - distance);

-                                        return true;

-                                }

-

-                                if (Vector3.Dot(delta, contactToCentre) >= 0.0f)

-                                        return false;

-

-                                // Moving towards the contact point -> collision

-                                point = contactPoint;

-                                timeOfImpact = 0.0f;

-                                return true;

-                        }

-                        return false;

-                }

-

-                private bool PointInTriangle(Vector3[] vertices, Vector3 normal, Vector3 p)

-                {

-                        Vector3 p1 = vertices[0];

-                        Vector3 p2 = vertices[1];

-                        Vector3 p3 = vertices[2];

-

-                        Vector3 edge1 = p2 - p1;

-                        Vector3 edge2 = p3 - p2;

-                        Vector3 edge3 = p1 - p3;

-

-                        Vector3 p1ToP = p - p1;

-                        Vector3 p2ToP = p - p2;

-                        Vector3 p3ToP = p - p3;

-

-                        Vector3 edge1Normal = Vector3.Cross(edge1, normal);

-                        Vector3 edge2Normal = Vector3.Cross(edge2, normal);

-                        Vector3 edge3Normal = Vector3.Cross(edge3, normal);

-

-                        float r1, r2, r3;

-                        r1 = Vector3.Dot(edge1Normal, p1ToP);

-                        r2 = Vector3.Dot(edge2Normal, p2ToP);

-                        r3 = Vector3.Dot(edge3Normal, p3ToP);

-                        if ((r1 > 0 && r2 > 0 && r3 > 0) ||

-                                 (r1 <= 0 && r2 <= 0 && r3 <= 0))

-                                return true;

-                        return false;

-                }

-

-                private bool FaceContains(Vector3 p, Vector3[] vertices, Vector3 normal)

-                {

-                        Vector3 lp = p;

-                        Vector3 lnormal = normal;

-                        return PointInTriangle(vertices, lnormal, lp);

-                }

-        }

+/*
+  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 SphereTriangleDetector : DiscreteCollisionDetectorInterface
+        {
+                private SphereShape _sphere;
+                private TriangleShape _triangle;
+                private const int MaxOverlap = 0;
+
+                public SphereTriangleDetector(SphereShape sphere, TriangleShape triangle)
+                {
+                        this._sphere = sphere;
+                        this._triangle = triangle;
+                }
+
+                public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw)
+                {
+                        Matrix transformA = input.TransformA;
+                        Matrix transformB = input.TransformB;
+
+                        Vector3 point = new Vector3();
+                        Vector3 normal = new Vector3();
+                        Single timeOfImpact = 1.0f;
+                        Single depth = 0.0f;
+
+                        //move sphere into triangle space
+                        Matrix sphereInTr = MathHelper.InverseTimes(transformB, transformA);
+
+                        if (Collide(sphereInTr.Translation, point, normal, depth, timeOfImpact))
+                                output.AddContactPoint(Vector3.TransformNormal(normal, transformB), Vector3.TransformNormal(point, transformB), depth);
+                }
+
+                /// <summary>
+                /// See also geometrictools.com
+                /// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
+                /// </summary>
+                /// <param name="from"></param>
+                /// <param name="to"></param>
+                /// <param name="p"></param>
+                /// <param name="nearest"></param>
+                /// <returns></returns>
+                private float SegmentSquareDistance(Vector3 from, Vector3 to, Vector3 point, Vector3 nearest)
+                {
+                        Vector3 diff = point - from;
+                        Vector3 v = to - from;
+                        float t = Vector3.Dot(v, diff);
+
+                        if (t > 0)
+                        {
+                                float dotVV = Vector3.Dot(v, v);
+                                if (t < dotVV)
+                                {
+                                        t /= dotVV;
+                                        diff -= t * v;
+                                }
+                                else
+                                {
+                                        t = 1;
+                                        diff -= v;
+                                }
+                        }
+                        else
+                                t = 0;
+
+                        nearest = from + t * v;
+                        return Vector3.Dot(diff, diff);
+                }
+
+                private bool Collide(Vector3 sphereCenter, Vector3 point, Vector3 resultNormal, float depth, float timeOfImpact)
+                {
+                        Vector3[] vertices = _triangle.Vertices;
+                        Vector3 c = sphereCenter;
+                        float r = _sphere.Radius;
+
+                        Vector3 delta = new Vector3();
+
+                        Vector3 normal = Vector3.Cross(vertices[1] - vertices[0], vertices[2] - vertices[0]);
+                        normal = Vector3.Normalize(normal);
+                        Vector3 p1ToCentre = c - vertices[0];
+                        float distanceFromPlane = Vector3.Dot(p1ToCentre, normal);
+
+                        if (distanceFromPlane < 0)
+                        {
+                                //triangle facing the other way
+                                distanceFromPlane *= -1;
+                                normal *= -1;
+                        }
+
+                        float contactMargin = PersistentManifold.ContactBreakingThreshold;
+                        bool isInsideContactPlane = distanceFromPlane < r + contactMargin;
+                        bool isInsideShellPlane = distanceFromPlane < r;
+
+                        float deltaDotNormal = Vector3.Dot(delta, normal);
+                        if (!isInsideShellPlane && deltaDotNormal >= 0.0f)
+                                return false;
+
+                        // Check for contact / intersection
+                        bool hasContact = false;
+                        Vector3 contactPoint = new Vector3();
+                        if (isInsideContactPlane)
+                        {
+                                if (FaceContains(c, vertices, normal))
+                                {
+                                        // Inside the contact wedge - touches a point on the shell plane
+                                        hasContact = true;
+                                        contactPoint = c - normal * distanceFromPlane;
+                                }
+                                else
+                                {
+                                        // Could be inside one of the contact capsules
+                                        float contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin);
+                                        Vector3 nearestOnEdge = new Vector3();
+                                        for (int i = 0; i < _triangle.EdgeCount; i++)
+                                        {
+                                                Vector3 pa, pb;
+                                                _triangle.GetEdge(i, out pa, out pb);
+
+                                                float distanceSqr = SegmentSquareDistance(pa, pb, c, nearestOnEdge);
+                                                if (distanceSqr < contactCapsuleRadiusSqr)
+                                                {
+                                                        // Yep, we're inside a capsule
+                                                        hasContact = true;
+                                                        contactPoint = nearestOnEdge;
+                                                }
+                                        }
+                                }
+                        }
+
+                        if (hasContact)
+                        {
+                                Vector3 contactToCentre = c - contactPoint;
+                                float distanceSqr = contactToCentre.LengthSquared();
+                                if (distanceSqr < (r - MaxOverlap) * (r - MaxOverlap))
+                                {
+                                        float distance = (float)Math.Sqrt(distanceSqr);
+                                        resultNormal = contactToCentre;
+                                        resultNormal = Vector3.Normalize(resultNormal);
+                                        point = contactPoint;
+                                        depth = -(r - distance);
+                                        return true;
+                                }
+
+                                if (Vector3.Dot(delta, contactToCentre) >= 0.0f)
+                                        return false;
+
+                                // Moving towards the contact point -> collision
+                                point = contactPoint;
+                                timeOfImpact = 0.0f;
+                                return true;
+                        }
+                        return false;
+                }
+
+                private bool PointInTriangle(Vector3[] vertices, Vector3 normal, Vector3 p)
+                {
+                        Vector3 p1 = vertices[0];
+                        Vector3 p2 = vertices[1];
+                        Vector3 p3 = vertices[2];
+
+                        Vector3 edge1 = p2 - p1;
+                        Vector3 edge2 = p3 - p2;
+                        Vector3 edge3 = p1 - p3;
+
+                        Vector3 p1ToP = p - p1;
+                        Vector3 p2ToP = p - p2;
+                        Vector3 p3ToP = p - p3;
+
+                        Vector3 edge1Normal = Vector3.Cross(edge1, normal);
+                        Vector3 edge2Normal = Vector3.Cross(edge2, normal);
+                        Vector3 edge3Normal = Vector3.Cross(edge3, normal);
+
+                        float r1, r2, r3;
+                        r1 = Vector3.Dot(edge1Normal, p1ToP);
+                        r2 = Vector3.Dot(edge2Normal, p2ToP);
+                        r3 = Vector3.Dot(edge3Normal, p3ToP);
+                        if ((r1 > 0 && r2 > 0 && r3 > 0) ||
+                                 (r1 <= 0 && r2 <= 0 && r3 <= 0))
+                                return true;
+                        return false;
+                }
+
+                private bool FaceContains(Vector3 p, Vector3[] vertices, Vector3 normal)
+                {
+                        Vector3 lp = p;
+                        Vector3 lnormal = normal;
+                        return PointInTriangle(vertices, lnormal, lp);
+                }
+        }
 }
\ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs
index a825fad..3c7daea 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs
@@ -1,151 +1,151 @@
-/*

-  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;

-

-namespace XnaDevRu.BulletX

-{

-        public class UnionFind : IDisposable

-        {

-                private List<Element> _elements = new List<Element>();

-

-                public int ElementCount

-                {

-                        get { return _elements.Count; }

-                }

-

-                public void SortIslands()

-                {

-                        for (int i = 0; i < _elements.Count; i++)

-                        {

-                                _elements[i].ID = Find(i);

-                                _elements[i].Size = i;

-                        }

-

-                        _elements.Sort(Sort);

-                }

-

-                private static int Sort(Element x, Element y)

-                {

-                        if (x.ID < y.ID) return -1;

-                        //else if (x.ID > y.ID) return 1;

-                        else return 0;

-                }

-

-                public void Reset(int number)

-                {

-                        Allocate(number);

-

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

-                        {

-                                Element element = new Element();

-                                element.ID = i; 

-                                element.Size = 1;

-                                _elements.Insert(i, element);

-                        }

-                }

-

-                public bool IsRoot(int index)

-                {

-                        return (_elements[index].Size == index);

-                }

-

-                public Element this[int index]

-                {

-                        get { return _elements[index]; }

-                }

-

-                public void Allocate(int number)

-                {

-                        //Does nothing

-                        _elements = new List<Element>(number);

-                }

-

-                public bool Find(int i, int j)

-                {

-                        return (Find(i) == Find(j));

-                }

-

-                public int Find(int i)

-                {

-                        while (i != _elements[i].ID)

-                        {

-                                //Element element = _elements[i];

-                                //element.ID = _elements[_elements[i].ID].ID;

-                                _elements[i].ID = _elements[_elements[i].ID].ID;

-                                i = _elements[i].ID;

-                        }

-

-                        return i;

-                }

-

-                public void Unite(int p, int q)

-                {

-                        int i = Find(p), j = Find(q);

-                        if (i == j)

-                                return;

-

-                        //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) )

-                        //if (_elements[i].Size < _elements[j].Size)

-                        //{

-                        //    Element element = _elements[i];

-                        //    element.ID = j;

-                        //    _elements[i] = element;

-

-                        //    element = _elements[j];

-                        //    element.Size += _elements[i].Size;

-                        //    _elements[j] = element;

-                        //}

-                        //else

-                        //{

-                        //    Element element = _elements[j];

-                        //    element.ID = i;

-                        //    _elements[j] = element;

-

-                        //    element = _elements[i];

-                        //    element.Size += _elements[j].Size;

-                        //    _elements[i] = element;

-                        //}

-                        _elements[i].ID = j;

-                        _elements[j].Size += _elements[i].Size;

-                }

-

-                #region IDisposable Members

-

-                public void Dispose()

-                {

-                        _elements.Clear();

-                }

-

-                #endregion

-        }

-

-        public class Element

-        {

-                private int _id;

-                private int _size;

-

-                public int ID { get { return _id; } set { _id = value; } }

-                public int Size { get { return _size; } set { _size = 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;
+
+namespace XnaDevRu.BulletX
+{
+        public class UnionFind : IDisposable
+        {
+                private List<Element> _elements = new List<Element>();
+
+                public int ElementCount
+                {
+                        get { return _elements.Count; }
+                }
+
+                public void SortIslands()
+                {
+                        for (int i = 0; i < _elements.Count; i++)
+                        {
+                                _elements[i].ID = Find(i);
+                                _elements[i].Size = i;
+                        }
+
+                        _elements.Sort(Sort);
+                }
+
+                private static int Sort(Element x, Element y)
+                {
+                        if (x.ID < y.ID) return -1;
+                        //else if (x.ID > y.ID) return 1;
+                        else return 0;
+                }
+
+                public void Reset(int number)
+                {
+                        Allocate(number);
+
+                        for (int i = 0; i < number; i++)
+                        {
+                                Element element = new Element();
+                                element.ID = i; 
+                                element.Size = 1;
+                                _elements.Insert(i, element);
+                        }
+                }
+
+                public bool IsRoot(int index)
+                {
+                        return (_elements[index].Size == index);
+                }
+
+                public Element this[int index]
+                {
+                        get { return _elements[index]; }
+                }
+
+                public void Allocate(int number)
+                {
+                        //Does nothing
+                        _elements = new List<Element>(number);
+                }
+
+                public bool Find(int i, int j)
+                {
+                        return (Find(i) == Find(j));
+                }
+
+                public int Find(int i)
+                {
+                        while (i != _elements[i].ID)
+                        {
+                                //Element element = _elements[i];
+                                //element.ID = _elements[_elements[i].ID].ID;
+                                _elements[i].ID = _elements[_elements[i].ID].ID;
+                                i = _elements[i].ID;
+                        }
+
+                        return i;
+                }
+
+                public void Unite(int p, int q)
+                {
+                        int i = Find(p), j = Find(q);
+                        if (i == j)
+                                return;
+
+                        //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) )
+                        //if (_elements[i].Size < _elements[j].Size)
+                        //{
+                        //    Element element = _elements[i];
+                        //    element.ID = j;
+                        //    _elements[i] = element;
+
+                        //    element = _elements[j];
+                        //    element.Size += _elements[i].Size;
+                        //    _elements[j] = element;
+                        //}
+                        //else
+                        //{
+                        //    Element element = _elements[j];
+                        //    element.ID = i;
+                        //    _elements[j] = element;
+
+                        //    element = _elements[i];
+                        //    element.Size += _elements[j].Size;
+                        //    _elements[i] = element;
+                        //}
+                        _elements[i].ID = j;
+                        _elements[j].Size += _elements[i].Size;
+                }
+
+                #region IDisposable Members
+
+                public void Dispose()
+                {
+                        _elements.Clear();
+                }
+
+                #endregion
+        }
+
+        public class Element
+        {
+                private int _id;
+                private int _size;
+
+                public int ID { get { return _id; } set { _id = value; } }
+                public int Size { get { return _size; } set { _size = value; } }
+        }
 }
\ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs
index b75c2b6..34f7442 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs
@@ -1,215 +1,215 @@
-/*

-  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>

-        /// BUSimplex1to4 implements feature based and implicit simplex of up to 4 vertices (tetrahedron, triangle, line, vertex).

-        /// </summary>

-        public class BUSimplex1to4 : PolyhedralConvexShape

-        {

-                private int _numVertices = 0;

-                private Vector3[] _vertices = new Vector3[4];

-

-                public BUSimplex1to4() { }

-

-                public BUSimplex1to4(Vector3 pointA)

-                {

-                        AddVertex(pointA);

-                }

-

-                public BUSimplex1to4(Vector3 pointA, Vector3 pointB)

-                {

-                        AddVertex(pointA);

-                        AddVertex(pointB);

-                }

-

-                public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC)

-                {

-                        AddVertex(pointA);

-                        AddVertex(pointB);

-                        AddVertex(pointC);

-                }

-

-                public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC, Vector3 pointD)

-                {

-                        AddVertex(pointA);

-                        AddVertex(pointB);

-                        AddVertex(pointC);

-                        AddVertex(pointD);

-                }

-

-                protected Vector3[] Vertices { get { return _vertices; } set { _vertices = value; } }

-

-                public override int VertexCount

-                {

-                        get

-                        {

-                                return _numVertices;

-                        }

-                }

-

-                public override int EdgeCount

-                {

-                        get

-                        {

-                                //euler formula, F-E+V = 2, so E = F+V-2

-                                switch (_numVertices)

-                                {

-                                        case 0: return 0;

-                                        case 1: return 0;

-                                        case 2: return 1;

-                                        case 3: return 3;

-                                        case 4: return 6;

-                                }

-                                return 0;

-                        }

-                }

-

-                public override int PlaneCount

-                {

-                        get

-                        {

-                                switch (_numVertices)

-                                {

-                                        case 0:

-                                                return 0;

-                                        case 1:

-                                                return 0;

-                                        case 2:

-                                                return 0;

-                                        case 3:

-                                                return 2;

-                                        case 4:

-                                                return 4;

-                                }

-                                return 0;

-                        }

-                }

-

-                public override BroadphaseNativeTypes ShapeType

-                {

-                        get

-                        {

-                                return BroadphaseNativeTypes.Tetrahedral;

-                        }

-                }

-

-                public override string Name

-                {

-                        get

-                        {

-                                return "BUSimplex1to4";

-                        }

-                }

-

-                public void AddVertex(Vector3 v)

-                {

-                        _vertices[_numVertices++] = v;

-                }

-

-                public void Reset()

-                {

-                        _numVertices = 0;

-                }

-

-                public override void GetEdge(int i, out Vector3 pa, out Vector3 pb)

-                {

-                        switch (_numVertices)

-                        {

-                                case 2:

-                                        pa = _vertices[0];

-                                        pb = _vertices[1];

-                                        return;

-                                case 3:

-                                        switch (i)

-                                        {

-                                                case 0:

-                                                        pa = _vertices[0];

-                                                        pb = _vertices[1];

-                                                        return;

-                                                case 1:

-                                                        pa = _vertices[1];

-                                                        pb = _vertices[2];

-                                                        return;

-                                                case 2:

-                                                        pa = _vertices[2];

-                                                        pb = _vertices[0];

-                                                        return;

-                                        }

-                                        break;

-                                case 4:

-                                        switch (i)

-                                        {

-                                                case 0:

-                                                        pa = _vertices[0];

-                                                        pb = _vertices[1];

-                                                        return;

-                                                case 1:

-                                                        pa = _vertices[1];

-                                                        pb = _vertices[2];

-                                                        return;

-                                                case 2:

-                                                        pa = _vertices[2];

-                                                        pb = _vertices[0];

-                                                        return;

-                                                case 3:

-                                                        pa = _vertices[0];

-                                                        pb = _vertices[3];

-                                                        return;

-                                                case 4:

-                                                        pa = _vertices[1];

-                                                        pb = _vertices[3];

-                                                        return;

-                                                case 5:

-                                                        pa = _vertices[2];

-                                                        pb = _vertices[3];

-                                                        return;

-                                        }

-                                        break;

-                        }

-

-                        pa = new Vector3();

-                        pb = new Vector3();

-                }

-

-                public override void GetVertex(int i, out Vector3 vtx)

-                {

-                        vtx = _vertices[i];

-                }

-

-                public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i)

-                {

-                        planeNormal = new Vector3();

-                        planeSupport = new Vector3();

-                }

-

-                public override bool IsInside(Vector3 pt, float tolerance)

-                {

-                        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>
+        /// BUSimplex1to4 implements feature based and implicit simplex of up to 4 vertices (tetrahedron, triangle, line, vertex).
+        /// </summary>
+        public class BUSimplex1to4 : PolyhedralConvexShape
+        {
+                private int _numVertices = 0;
+                private Vector3[] _vertices = new Vector3[4];
+
+                public BUSimplex1to4() { }
+
+                public BUSimplex1to4(Vector3 pointA)
+                {
+                        AddVertex(pointA);
+                }
+
+                public BUSimplex1to4(Vector3 pointA, Vector3 pointB)
+                {
+                        AddVertex(pointA);
+                        AddVertex(pointB);
+                }
+
+                public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC)
+                {
+                        AddVertex(pointA);
+                        AddVertex(pointB);
+                        AddVertex(pointC);
+                }
+
+                public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC, Vector3 pointD)
+                {
+                        AddVertex(pointA);
+                        AddVertex(pointB);
+                        AddVertex(pointC);
+                        AddVertex(pointD);
+                }
+
+                protected Vector3[] Vertices { get { return _vertices; } set { _vertices = value; } }
+
+                public override int VertexCount
+                {
+                        get
+                        {
+                                return _numVertices;
+                        }
+                }
+
+                public override int EdgeCount
+                {
+                        get
+                        {
+                                //euler formula, F-E+V = 2, so E = F+V-2
+                                switch (_numVertices)
+                                {
+                                        case 0: return 0;
+                                        case 1: return 0;
+                                        case 2: return 1;
+                                        case 3: return 3;
+                                        case 4: return 6;
+                                }
+                                return 0;
+                        }
+                }
+
+                public override int PlaneCount
+                {
+                        get
+                        {
+                                switch (_numVertices)
+                                {
+                                        case 0:
+                                                return 0;
+                                        case 1:
+                                                return 0;
+                                        case 2:
+                                                return 0;
+                                        case 3:
+                                                return 2;
+                                        case 4:
+                                                return 4;
+                                }
+                                return 0;
+                        }
+                }
+
+                public override BroadphaseNativeTypes ShapeType
+                {
+                        get
+                        {
+                                return BroadphaseNativeTypes.Tetrahedral;
+                        }
+                }
+
+                public override string Name
+                {
+                        get
+                        {
+                                return "BUSimplex1to4";
+                        }
+                }
+
+                public void AddVertex(Vector3 v)
+                {
+                        _vertices[_numVertices++] = v;
+                }
+
+                public void Reset()
+                {
+                        _numVertices = 0;
+                }
+
+                public override void GetEdge(int i, out Vector3 pa, out Vector3 pb)
+                {
+                        switch (_numVertices)
+                        {
+                                case 2:
+                                        pa = _vertices[0];
+                                        pb = _vertices[1];
+                                        return;
+                                case 3:
+                                        switch (i)
+                                        {
+                                                case 0:
+                                                        pa = _vertices[0];
+                                                        pb = _vertices[1];
+                                                        return;
+                                                case 1:
+                                                        pa = _vertices[1];
+                                                        pb = _vertices[2];
+                                                        return;
+                                                case 2:
+                                                        pa = _vertices[2];
+                                                        pb = _vertices[0];
+                                                        return;
+                                        }
+                                        break;
+                                case 4:
+                                        switch (i)
+                                        {
+                                                case 0:
+                                                        pa = _vertices[0];
+                                                        pb = _vertices[1];
+                                                        return;
+                                                case 1:
+                                                        pa = _vertices[1];
+                                                        pb = _vertices[2];
+                                                        return;
+                                                case 2:
+                                                        pa = _vertices[2];
+                                                        pb = _vertices[0];
+                                                        return;
+                                                case 3:
+                                                        pa = _vertices[0];
+                                                        pb = _vertices[3];
+                                                        return;
+                                                case 4:
+                                                        pa = _vertices[1];
+                                                        pb = _vertices[3];
+                                                        return;
+                                                case 5:
+                                                        pa = _vertices[2];
+                                                        pb = _vertices[3];
+                                                        return;
+                                        }
+                                        break;
+                        }
+
+                        pa = new Vector3();
+                        pb = new Vector3();
+                }
+
+                public override void GetVertex(int i, out Vector3 vtx)
+                {
+                        vtx = _vertices[i];
+                }
+
+                public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i)
+                {
+                        planeNormal = new Vector3();
+                        planeSupport = new Vector3();
+                }
+
+                public override bool IsInside(Vector3 pt, float tolerance)
+                {
+                        return false;
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs
index ba6f3b7..7fab5c7 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs
@@ -1,316 +1,316 @@
-/*

-  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 BoxShape : PolyhedralConvexShape

-        {

-                public BoxShape(Vector3 boxHalfExtents)

-                {

-                        ImplicitShapeDimensions = boxHalfExtents;

-                }

-

-                public override int VertexCount

-                {

-                        get

-                        {

-                                return 8;

-                        }

-                }

-

-                public override int EdgeCount

-                {

-                        get

-                        {

-                                return 12;

-                        }

-                }

-

-                public override BroadphaseNativeTypes ShapeType

-                {

-                        get

-                        {

-                                return BroadphaseNativeTypes.Box;

-                        }

-                }

-

-                public override string Name

-                {

-                        get

-                        {

-                                return "Box";

-                        }

-                }

-

-                public override int PreferredPenetrationDirectionsCount

-                {

-                        get

-                        {

-                                return 6;

-                        }

-                }

-

-                public override int PlaneCount

-                {

-                        get

-                        {

-                                return 6;

-                        }

-                }

-

-                public Vector3 HalfExtents { get { return ImplicitShapeDimensions * LocalScaling; } }

-

-                public override void GetEdge(int i, out Vector3 pa, out Vector3 pb)

-                {

-                        int edgeVert0 = 0;

-                        int edgeVert1 = 0;

-

-                        switch (i)

-                        {

-                                case 0:

-                                        edgeVert0 = 0;

-                                        edgeVert1 = 1;

-                                        break;

-                                case 1:

-                                        edgeVert0 = 0;

-                                        edgeVert1 = 2;

-                                        break;

-                                case 2:

-                                        edgeVert0 = 1;

-                                        edgeVert1 = 3;

-

-                                        break;

-                                case 3:

-                                        edgeVert0 = 2;

-                                        edgeVert1 = 3;

-                                        break;

-                                case 4:

-                                        edgeVert0 = 0;

-                                        edgeVert1 = 4;

-                                        break;

-                                case 5:

-                                        edgeVert0 = 1;

-                                        edgeVert1 = 5;

-

-                                        break;

-                                case 6:

-                                        edgeVert0 = 2;

-                                        edgeVert1 = 6;

-                                        break;

-                                case 7:

-                                        edgeVert0 = 3;

-                                        edgeVert1 = 7;

-                                        break;

-                                case 8:

-                                        edgeVert0 = 4;

-                                        edgeVert1 = 5;

-                                        break;

-                                case 9:

-                                        edgeVert0 = 4;

-                                        edgeVert1 = 6;

-                                        break;

-                                case 10:

-                                        edgeVert0 = 5;

-                                        edgeVert1 = 7;

-                                        break;

-                                case 11:

-                                        edgeVert0 = 6;

-                                        edgeVert1 = 7;

-                                        break;

-                                default:

-                                        throw new BulletException();

-

-                        }

-

-                        GetVertex(edgeVert0, out pa);

-                        GetVertex(edgeVert1, out pb);

-                }

-

-                public override void GetVertex(int i, out Vector3 vtx)

-                {

-                        Vector3 halfExtents = HalfExtents;

-

-                        vtx = new Vector3(

-                                        halfExtents.X * (1 - (i & 1)) - halfExtents.X * (i & 1),

-                                        halfExtents.Y * (1 - ((i & 2) >> 1)) - halfExtents.Y * ((i & 2) >> 1),

-                                        halfExtents.Z * (1 - ((i & 4) >> 2)) - halfExtents.Z * ((i & 4) >> 2));

-                }

-

-                public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i)

-                {

-                        //this plane might not be aligned...

-                        Vector4 plane;

-                        GetPlaneEquation(out plane, i);

-                        planeNormal = new Vector3(plane.X, plane.Y, plane.Z);

-                        planeSupport = LocalGetSupportingVertex(-planeNormal);

-                }

-

-                public override bool IsInside(Vector3 pt, float tolerance)

-                {

-                        Vector3 halfExtents = HalfExtents;

-

-                        //btScalar minDist = 2*tolerance;

-

-                        bool result =   (pt.X <= ( halfExtents.X + tolerance)) &&

-                                                        (pt.X >= (-halfExtents.X - tolerance)) &&

-                                                        (pt.Y <= ( halfExtents.Y + tolerance)) &&

-                                                        (pt.Y >= (-halfExtents.Y - tolerance)) &&

-                                                        (pt.Z <= ( halfExtents.Z + tolerance)) &&

-                                                        (pt.Z >= (-halfExtents.Z - tolerance));

-

-                        return result;

-                }

-

-                public override Vector3 LocalGetSupportingVertex(Vector3 vec)

-                {

-                        Vector3 halfExtents = HalfExtents;

-

-                        return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X,

-                                                                vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y,

-                                                                vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z);

-                }

-

-                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)

-                {

-                        Vector3 halfExtents = HalfExtents;

-                        Vector3 margin = new Vector3(Margin, Margin, Margin);

-                        halfExtents -= margin;

-

-                        return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X,

-                                                                vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y,

-                                                                vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z);

-                }

-

-                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)

-                {

-                        Vector3 halfExtents = HalfExtents;

-                        Vector3 margin = new Vector3(Margin, Margin, Margin);

-                        halfExtents -= margin;

-

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

-                        {

-                                Vector3 vec = vectors[i];

-                                supportVerticesOut[i] = new Vector3(vec.X < 0.0f ? -halfExtents.X : halfExtents.X,

-                                                                                                        vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y,

-                                                                                                        vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z);

-                        }

-                }

-

-                public virtual void GetPlaneEquation(out Vector4 plane, int i)

-                {

-                        Vector3 halfExtents = HalfExtents;

-

-                        switch (i)

-                        {

-                                case 0:

-                                        plane = new Vector4(1, 0, 0, 0);

-                                        plane.W = -halfExtents.X;

-                                        break;

-                                case 1:

-                                        plane = new Vector4(-1, 0, 0, 0);

-                                        plane.W = -halfExtents.X;

-                                        break;

-                                case 2:

-                                        plane = new Vector4(0, 1, 0, 0);

-                                        plane.W = -halfExtents.Y;

-                                        break;

-                                case 3:

-                                        plane = new Vector4(0, -1, 0, 0);

-                                        plane.W = -halfExtents.Y;

-                                        break;

-                                case 4:

-                                        plane = new Vector4(0, 0, 1, 0);

-                                        plane.W = -halfExtents.Z;

-                                        break;

-                                case 5:

-                                        plane = new Vector4(0, 0, -1, 0);

-                                        plane.W = -halfExtents.Z;

-                                        break;

-                                default:

-                                        throw new BulletException();

-                        }

-                }

-

-                public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector)

-                {

-                        switch (index)

-                        {

-                                case 0:

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

-                                        break;

-                                case 1:

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

-                                        break;

-                                case 2:

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

-                                        break;

-                                case 3:

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

-                                        break;

-                                case 4:

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

-                                        break;

-                                case 5:

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

-                                        break;

-                                default:

-                                        throw new BulletException();

-                        }

-                }

-

-                public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)

-                {

-                        Vector3 halfExtents = HalfExtents;

-

-                        Matrix abs_b = MathHelper.Absolute(t);

-                        Vector3 center = t.Translation;

-                        Vector3 row1 = new Vector3(abs_b.M11, abs_b.M12, abs_b.M13);

-                        Vector3 row2 = new Vector3(abs_b.M21, abs_b.M22, abs_b.M23);

-                        Vector3 row3 = new Vector3(abs_b.M31, abs_b.M32, abs_b.M33);

-                        Vector3 extent = new Vector3(Vector3.Dot(row1, halfExtents),

-                                                                                 Vector3.Dot(row2, halfExtents),

-                                                                                 Vector3.Dot(row3, halfExtents));

-                        extent += new Vector3(Margin, Margin, Margin);

-

-                        aabbMin = center - extent;

-                        aabbMax = center + extent;

-                }

-

-                public override void CalculateLocalInertia(float mass, out Vector3 inertia)

-                {

-                        Vector3 halfExtents = HalfExtents;

-

-                        float lx = 2f * (halfExtents.X);

-                        float ly = 2f * (halfExtents.Y);

-                        float lz = 2f * (halfExtents.Z);

-

-                        inertia = new Vector3();

-                        inertia.X = mass / (12.0f) * (ly * ly + lz * lz);

-                        inertia.Y = mass / (12.0f) * (lx * lx + lz * lz);

-                        inertia.Z = mass / (12.0f) * (lx * lx + ly * ly);

-                }

-        }

-}

+/*
+  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 BoxShape : PolyhedralConvexShape
+        {
+                public BoxShape(Vector3 boxHalfExtents)
+                {
+                        ImplicitShapeDimensions = boxHalfExtents;
+                }
+
+                public override int VertexCount
+                {
+                        get
+                        {
+                                return 8;
+                        }
+                }
+
+                public override int EdgeCount
+                {
+                        get
+                        {
+                                return 12;
+                        }
+                }
+
+                public override BroadphaseNativeTypes ShapeType
+                {
+                        get
+                        {
+                                return BroadphaseNativeTypes.Box;
+                        }
+                }
+
+                public override string Name
+                {
+                        get
+                        {
+                                return "Box";
+                        }
+                }
+
+                public override int PreferredPenetrationDirectionsCount
+                {
+                        get
+                        {
+                                return 6;
+                        }
+                }
+
+                public override int PlaneCount
+                {
+                        get
+                        {
+                                return 6;
+                        }
+                }
+
+                public Vector3 HalfExtents { get { return ImplicitShapeDimensions * LocalScaling; } }
+
+                public override void GetEdge(int i, out Vector3 pa, out Vector3 pb)
+                {
+                        int edgeVert0 = 0;
+                        int edgeVert1 = 0;
+
+                        switch (i)
+                        {
+                                case 0:
+                                        edgeVert0 = 0;
+                                        edgeVert1 = 1;
+                                        break;
+                                case 1:
+                                        edgeVert0 = 0;
+                                        edgeVert1 = 2;
+                                        break;
+                                case 2:
+                                        edgeVert0 = 1;
+                                        edgeVert1 = 3;
+
+                                        break;
+                                case 3:
+                                        edgeVert0 = 2;
+                                        edgeVert1 = 3;
+                                        break;
+                                case 4:
+                                        edgeVert0 = 0;
+                                        edgeVert1 = 4;
+                                        break;
+                                case 5:
+                                        edgeVert0 = 1;
+                                        edgeVert1 = 5;
+
+                                        break;
+                                case 6:
+                                        edgeVert0 = 2;
+                                        edgeVert1 = 6;
+                                        break;
+                                case 7:
+                                        edgeVert0 = 3;
+                                        edgeVert1 = 7;
+                                        break;
+                                case 8:
+                                        edgeVert0 = 4;
+                                        edgeVert1 = 5;
+                                        break;
+                                case 9:
+                                        edgeVert0 = 4;
+                                        edgeVert1 = 6;
+                                        break;
+                                case 10:
+                                        edgeVert0 = 5;
+                                        edgeVert1 = 7;
+                                        break;
+                                case 11:
+                                        edgeVert0 = 6;
+                                        edgeVert1 = 7;
+                                        break;
+                                default:
+                                        throw new BulletException();
+
+                        }
+
+                        GetVertex(edgeVert0, out pa);
+                        GetVertex(edgeVert1, out pb);
+                }
+
+                public override void GetVertex(int i, out Vector3 vtx)
+                {
+                        Vector3 halfExtents = HalfExtents;
+
+                        vtx = new Vector3(
+                                        halfExtents.X * (1 - (i & 1)) - halfExtents.X * (i & 1),
+                                        halfExtents.Y * (1 - ((i & 2) >> 1)) - halfExtents.Y * ((i & 2) >> 1),
+                                        halfExtents.Z * (1 - ((i & 4) >> 2)) - halfExtents.Z * ((i & 4) >> 2));
+                }
+
+                public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i)
+                {
+                        //this plane might not be aligned...
+                        Vector4 plane;
+                        GetPlaneEquation(out plane, i);
+                        planeNormal = new Vector3(plane.X, plane.Y, plane.Z);
+                        planeSupport = LocalGetSupportingVertex(-planeNormal);
+                }
+
+                public override bool IsInside(Vector3 pt, float tolerance)
+                {
+                        Vector3 halfExtents = HalfExtents;
+
+                        //btScalar minDist = 2*tolerance;
+
+                        bool result =   (pt.X <= ( halfExtents.X + tolerance)) &&
+                                                        (pt.X >= (-halfExtents.X - tolerance)) &&
+                                                        (pt.Y <= ( halfExtents.Y + tolerance)) &&
+                                                        (pt.Y >= (-halfExtents.Y - tolerance)) &&
+                                                        (pt.Z <= ( halfExtents.Z + tolerance)) &&
+                                                        (pt.Z >= (-halfExtents.Z - tolerance));
+
+                        return result;
+                }
+
+                public override Vector3 LocalGetSupportingVertex(Vector3 vec)
+                {
+                        Vector3 halfExtents = HalfExtents;
+
+                        return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X,
+                                                                vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y,
+                                                                vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z);
+                }
+
+                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
+                {
+                        Vector3 halfExtents = HalfExtents;
+                        Vector3 margin = new Vector3(Margin, Margin, Margin);
+                        halfExtents -= margin;
+
+                        return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X,
+                                                                vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y,
+                                                                vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z);
+                }
+
+                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
+                {
+                        Vector3 halfExtents = HalfExtents;
+                        Vector3 margin = new Vector3(Margin, Margin, Margin);
+                        halfExtents -= margin;
+
+                        for (int i = 0; i < vectors.Length; i++)
+                        {
+                                Vector3 vec = vectors[i];
+                                supportVerticesOut[i] = new Vector3(vec.X < 0.0f ? -halfExtents.X : halfExtents.X,
+                                                                                                        vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y,
+                                                                                                        vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z);
+                        }
+                }
+
+                public virtual void GetPlaneEquation(out Vector4 plane, int i)
+                {
+                        Vector3 halfExtents = HalfExtents;
+
+                        switch (i)
+                        {
+                                case 0:
+                                        plane = new Vector4(1, 0, 0, 0);
+                                        plane.W = -halfExtents.X;
+                                        break;
+                                case 1:
+                                        plane = new Vector4(-1, 0, 0, 0);
+                                        plane.W = -halfExtents.X;
+                                        break;
+                                case 2:
+                                        plane = new Vector4(0, 1, 0, 0);
+                                        plane.W = -halfExtents.Y;
+                                        break;
+                                case 3:
+                                        plane = new Vector4(0, -1, 0, 0);
+                                        plane.W = -halfExtents.Y;
+                                        break;
+                                case 4:
+                                        plane = new Vector4(0, 0, 1, 0);
+                                        plane.W = -halfExtents.Z;
+                                        break;
+                                case 5:
+                                        plane = new Vector4(0, 0, -1, 0);
+                                        plane.W = -halfExtents.Z;
+                                        break;
+                                default:
+                                        throw new BulletException();
+                        }
+                }
+
+                public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector)
+                {
+                        switch (index)
+                        {
+                                case 0:
+                                        penetrationVector = new Vector3(1, 0, 0);
+                                        break;
+                                case 1:
+                                        penetrationVector = new Vector3(-1, 0, 0);
+                                        break;
+                                case 2:
+                                        penetrationVector = new Vector3(0, 1, 0);
+                                        break;
+                                case 3:
+                                        penetrationVector = new Vector3(0, -1, 0);
+                                        break;
+                                case 4:
+                                        penetrationVector = new Vector3(0, 0, 1);
+                                        break;
+                                case 5:
+                                        penetrationVector = new Vector3(0, 0, -1);
+                                        break;
+                                default:
+                                        throw new BulletException();
+                        }
+                }
+
+                public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
+                {
+                        Vector3 halfExtents = HalfExtents;
+
+                        Matrix abs_b = MathHelper.Absolute(t);
+                        Vector3 center = t.Translation;
+                        Vector3 row1 = new Vector3(abs_b.M11, abs_b.M12, abs_b.M13);
+                        Vector3 row2 = new Vector3(abs_b.M21, abs_b.M22, abs_b.M23);
+                        Vector3 row3 = new Vector3(abs_b.M31, abs_b.M32, abs_b.M33);
+                        Vector3 extent = new Vector3(Vector3.Dot(row1, halfExtents),
+                                                                                 Vector3.Dot(row2, halfExtents),
+                                                                                 Vector3.Dot(row3, halfExtents));
+                        extent += new Vector3(Margin, Margin, Margin);
+
+                        aabbMin = center - extent;
+                        aabbMax = center + extent;
+                }
+
+                public override void CalculateLocalInertia(float mass, out Vector3 inertia)
+                {
+                        Vector3 halfExtents = HalfExtents;
+
+                        float lx = 2f * (halfExtents.X);
+                        float ly = 2f * (halfExtents.Y);
+                        float lz = 2f * (halfExtents.Z);
+
+                        inertia = new Vector3();
+                        inertia.X = mass / (12.0f) * (ly * ly + lz * lz);
+                        inertia.Y = mass / (12.0f) * (lx * lx + lz * lz);
+                        inertia.Z = mass / (12.0f) * (lx * lx + ly * ly);
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs
index 00247a0..03e1e44 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs
@@ -1,83 +1,83 @@
-/*

-  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

-{

-    class MyNodeOverlapCallback : INodeOverlapCallback

-    {

-        StridingMeshInterface _meshInterface;

-        ITriangleCallback _callback;

-        Vector3[] _triangle = new Vector3[3];

-

-        public MyNodeOverlapCallback(ITriangleCallback callback, StridingMeshInterface meshInterface)

-        {

-            _meshInterface = meshInterface;

-            _callback = callback;

-        }

-

-        public void ProcessNode(OptimizedBvhNode node)

-        {

-            List<Vector3> verts;

-            List<int> indicies;

-            int numtriangles;

-

-            _meshInterface.GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, node.SubPart);

-            Vector3 meshScaling = _meshInterface.Scaling;

-

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

-            {

-                _triangle[j] = verts[indicies[j + node.TriangleIndex * 3]] * meshScaling;

-            }

-

-            _callback.ProcessTriangle(_triangle, node.SubPart, node.TriangleIndex);

-            _meshInterface.UnLockReadOnlyVertexBase(node.SubPart);

-        }

-    }

-

-    public class BvhTriangleMeshShape : TriangleMeshShape

-    {

-        OptimizedBvh _bvh = new OptimizedBvh();

-

-        public BvhTriangleMeshShape(StridingMeshInterface meshInterface) : base(meshInterface)

-        {

-            _bvh.Build(meshInterface);

-        }

-

-        public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)

-        {

-            MyNodeOverlapCallback myNodeCallback = new MyNodeOverlapCallback(callback, MeshInterface);

-            _bvh.ReportAabbOverlappingNodex(myNodeCallback, aabbMin, aabbMax);

-        }

-

-        public override string Name

-        {

-            get

-            {

-                return "BvhTriangleMesh";

-            }

-        }

-    }

-}

+/*
+  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
+{
+    class MyNodeOverlapCallback : INodeOverlapCallback
+    {
+        StridingMeshInterface _meshInterface;
+        ITriangleCallback _callback;
+        Vector3[] _triangle = new Vector3[3];
+
+        public MyNodeOverlapCallback(ITriangleCallback callback, StridingMeshInterface meshInterface)
+        {
+            _meshInterface = meshInterface;
+            _callback = callback;
+        }
+
+        public void ProcessNode(OptimizedBvhNode node)
+        {
+            List<Vector3> verts;
+            List<int> indicies;
+            int numtriangles;
+
+            _meshInterface.GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, node.SubPart);
+            Vector3 meshScaling = _meshInterface.Scaling;
+
+            for (int j = 0; j < 3; j++)
+            {
+                _triangle[j] = verts[indicies[j + node.TriangleIndex * 3]] * meshScaling;
+            }
+
+            _callback.ProcessTriangle(_triangle, node.SubPart, node.TriangleIndex);
+            _meshInterface.UnLockReadOnlyVertexBase(node.SubPart);
+        }
+    }
+
+    public class BvhTriangleMeshShape : TriangleMeshShape
+    {
+        OptimizedBvh _bvh = new OptimizedBvh();
+
+        public BvhTriangleMeshShape(StridingMeshInterface meshInterface) : base(meshInterface)
+        {
+            _bvh.Build(meshInterface);
+        }
+
+        public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
+        {
+            MyNodeOverlapCallback myNodeCallback = new MyNodeOverlapCallback(callback, MeshInterface);
+            _bvh.ReportAabbOverlappingNodex(myNodeCallback, aabbMin, aabbMax);
+        }
+
+        public override string Name
+        {
+            get
+            {
+                return "BvhTriangleMesh";
+            }
+        }
+    }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs
index a9ce3be..1a4fac2 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs
@@ -1,148 +1,148 @@
-/*

-  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>

-        /// CollisionShape provides generic interface for collidable objects

-        /// </summary>

-        public abstract class CollisionShape

-        {

-                //debugging support

-                private string _tempDebug;

-

-                public abstract string Name { get; }

-                public string ExtraDebugInfo { get { return _tempDebug; } set { _tempDebug = value; } }

-

-                public bool IsPolyhedral

-                {

-                        get

-                        {

-                                return BroadphaseProxy.IsPolyhedral(ShapeType);

-                        }

-                }

-

-                public bool IsConvex

-                {

-                        get

-                        {

-                                return BroadphaseProxy.IsConvex(ShapeType);

-                        }

-                }

-                public bool IsConcave

-                {

-                        get

-                        {

-                                return BroadphaseProxy.IsConcave(ShapeType);

-                        }

-                }

-                public bool IsCompound

-                {

-                        get

-                        {

-                                return BroadphaseProxy.IsCompound(ShapeType);

-                        }

-                }

-

-                //isInfinite is used to catch simulation error (aabb check)

-                public bool IsInfinite

-                {

-                        get

-                        {

-                                return BroadphaseProxy.IsInfinite(ShapeType);

-                        }

-                }

-

-                public abstract float Margin { get; set; }

-                public abstract Vector3 LocalScaling { get; set; }

-                public abstract BroadphaseNativeTypes ShapeType { get; }

-                

-

-                public virtual void GetBoundingSphere(out Vector3 center, out float radius)

-                {

-                        Matrix tr = Matrix.Identity;

-                        Vector3 aabbMin, aabbMax;

-

-                        GetAabb(tr, out aabbMin, out aabbMax);

-

-                        radius = (aabbMax - aabbMin).Length() * 0.5f;

-                        center = (aabbMin + aabbMax) * 0.5f;

-                }

-

-                public virtual float GetAngularMotionDisc()

-                {

-                        Vector3 center;

-                        float disc;

-                        GetBoundingSphere(out center, out disc);

-                        disc += center.Length();

-                        return disc;

-                }

-

-                //calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep)

-                //result is conservative

-                public void CalculateTemporalAabb(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, out Vector3 temporalAabbMin, out Vector3 temporalAabbMax)

-                {

-                        //start with static aabb

-                        GetAabb(currentTransform, out temporalAabbMin, out temporalAabbMax);

-

-                        float temporalAabbMaxx = temporalAabbMax.X;

-                        float temporalAabbMaxy = temporalAabbMax.Y;

-                        float temporalAabbMaxz = temporalAabbMax.Z;

-                        float temporalAabbMinx = temporalAabbMin.X;

-                        float temporalAabbMiny = temporalAabbMin.Y;

-                        float temporalAabbMinz = temporalAabbMin.Z;

-

-                        // add linear motion

-                        Vector3 linMotion = linearVelocity * timeStep;

-                        //todo: simd would have a vector max/min operation, instead of per-element access

-                        if (linMotion.X > 0)

-                                temporalAabbMaxx += linMotion.X;

-                        else

-                                temporalAabbMinx += linMotion.X;

-                        if (linMotion.Y > 0)

-                                temporalAabbMaxy += linMotion.Y;

-                        else

-                                temporalAabbMiny += linMotion.Y;

-                        if (linMotion.Z > 0)

-                                temporalAabbMaxz += linMotion.Z;

-                        else

-                                temporalAabbMinz += linMotion.Z;

-

-                        //add conservative angular motion

-                        float angularMotion = angularVelocity.Length() * GetAngularMotionDisc() * timeStep;

-                        Vector3 angularMotion3d = new Vector3(angularMotion, angularMotion, angularMotion);

-                        temporalAabbMin = new Vector3(temporalAabbMinx, temporalAabbMiny, temporalAabbMinz);

-                        temporalAabbMax = new Vector3(temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz);

-

-                        temporalAabbMin -= angularMotion3d;

-                        temporalAabbMax += angularMotion3d;

-                }

-

-                public abstract void GetAabb(Matrix transform, out Vector3 aabbMin, out Vector3 aabbMax);

-

-                public abstract void CalculateLocalInertia(float mass, out Vector3 inertia);

-        }

-}

+/*
+  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>
+        /// CollisionShape provides generic interface for collidable objects
+        /// </summary>
+        public abstract class CollisionShape
+        {
+                //debugging support
+                private string _tempDebug;
+
+                public abstract string Name { get; }
+                public string ExtraDebugInfo { get { return _tempDebug; } set { _tempDebug = value; } }
+
+                public bool IsPolyhedral
+                {
+                        get
+                        {
+                                return BroadphaseProxy.IsPolyhedral(ShapeType);
+                        }
+                }
+
+                public bool IsConvex
+                {
+                        get
+                        {
+                                return BroadphaseProxy.IsConvex(ShapeType);
+                        }
+                }
+                public bool IsConcave
+                {
+                        get
+                        {
+                                return BroadphaseProxy.IsConcave(ShapeType);
+                        }
+                }
+                public bool IsCompound
+                {
+                        get
+                        {
+                                return BroadphaseProxy.IsCompound(ShapeType);
+                        }
+                }
+
+                //isInfinite is used to catch simulation error (aabb check)
+                public bool IsInfinite
+                {
+                        get
+                        {
+                                return BroadphaseProxy.IsInfinite(ShapeType);
+                        }
+                }
+
+                public abstract float Margin { get; set; }
+                public abstract Vector3 LocalScaling { get; set; }
+                public abstract BroadphaseNativeTypes ShapeType { get; }
+                
+
+                public virtual void GetBoundingSphere(out Vector3 center, out float radius)
+                {
+                        Matrix tr = Matrix.Identity;
+                        Vector3 aabbMin, aabbMax;
+
+                        GetAabb(tr, out aabbMin, out aabbMax);
+
+                        radius = (aabbMax - aabbMin).Length() * 0.5f;
+                        center = (aabbMin + aabbMax) * 0.5f;
+                }
+
+                public virtual float GetAngularMotionDisc()
+                {
+                        Vector3 center;
+                        float disc;
+                        GetBoundingSphere(out center, out disc);
+                        disc += center.Length();
+                        return disc;
+                }
+
+                //calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep)
+                //result is conservative
+                public void CalculateTemporalAabb(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, out Vector3 temporalAabbMin, out Vector3 temporalAabbMax)
+                {
+                        //start with static aabb
+                        GetAabb(currentTransform, out temporalAabbMin, out temporalAabbMax);
+
+                        float temporalAabbMaxx = temporalAabbMax.X;
+                        float temporalAabbMaxy = temporalAabbMax.Y;
+                        float temporalAabbMaxz = temporalAabbMax.Z;
+                        float temporalAabbMinx = temporalAabbMin.X;
+                        float temporalAabbMiny = temporalAabbMin.Y;
+                        float temporalAabbMinz = temporalAabbMin.Z;
+
+                        // add linear motion
+                        Vector3 linMotion = linearVelocity * timeStep;
+                        //todo: simd would have a vector max/min operation, instead of per-element access
+                        if (linMotion.X > 0)
+                                temporalAabbMaxx += linMotion.X;
+                        else
+                                temporalAabbMinx += linMotion.X;
+                        if (linMotion.Y > 0)
+                                temporalAabbMaxy += linMotion.Y;
+                        else
+                                temporalAabbMiny += linMotion.Y;
+                        if (linMotion.Z > 0)
+                                temporalAabbMaxz += linMotion.Z;
+                        else
+                                temporalAabbMinz += linMotion.Z;
+
+                        //add conservative angular motion
+                        float angularMotion = angularVelocity.Length() * GetAngularMotionDisc() * timeStep;
+                        Vector3 angularMotion3d = new Vector3(angularMotion, angularMotion, angularMotion);
+                        temporalAabbMin = new Vector3(temporalAabbMinx, temporalAabbMiny, temporalAabbMinz);
+                        temporalAabbMax = new Vector3(temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz);
+
+                        temporalAabbMin -= angularMotion3d;
+                        temporalAabbMax += angularMotion3d;
+                }
+
+                public abstract void GetAabb(Matrix transform, out Vector3 aabbMin, out Vector3 aabbMax);
+
+                public abstract void CalculateLocalInertia(float mass, out Vector3 inertia);
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs
index 08aeec8..bd13ffd 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs
@@ -1,183 +1,183 @@
-/*

-  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>

-        /// CompoundShape allows to store multiple other CollisionShapes

-        /// This allows for concave collision objects. This is more general then the Static Concave TriangleMeshShape.

-        /// </summary>

-        public class CompoundShape : CollisionShape

-        {

-                private List<Matrix> _childTransforms = new List<Matrix>();

-                private List<CollisionShape> _childShapes = new List<CollisionShape>();

-                private Vector3 _localAabbMin;

-                private Vector3 _localAabbMax;

-

-                private OptimizedBvh _aabbTree;

-                private float _collisionMargin;

-                private Vector3 _localScaling;

-

-                public CompoundShape()

-                {

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

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

-                        _aabbTree = null;

-                        _collisionMargin = 0f;

-                        _localScaling = new Vector3(1f, 1f, 1f);

-                }

-

-                public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)

-                {

-                        Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin);

-                        Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin);

-

-                        Matrix abs_b = MathHelper.Absolute(t);

-

-                        Vector3 row1 = new Vector3(abs_b.M11, abs_b.M12, abs_b.M13);

-                        Vector3 row2 = new Vector3(abs_b.M21, abs_b.M22, abs_b.M23);

-                        Vector3 row3 = new Vector3(abs_b.M31, abs_b.M32, abs_b.M33);

-

-                        Vector3 center = new Vector3(Vector3.Dot(row1, localCenter) + t.Translation.X,

-                                                                                 Vector3.Dot(row2, localCenter) + t.Translation.Y,

-                                                                                 Vector3.Dot(row3, localCenter) + t.Translation.Z);

-

-                        Vector3 extent = new Vector3(Vector3.Dot(row1, localHalfExtents),

-                                                                                 Vector3.Dot(row2, localHalfExtents),

-                                                                                 Vector3.Dot(row3, localHalfExtents));

-

-                        aabbMin = center - extent;

-                        aabbMax = center + extent;

-                }

-

-                public override BroadphaseNativeTypes ShapeType

-                {

-                        get

-                        {

-                                return BroadphaseNativeTypes.Compound;

-                        }

-                }

-

-                public override Vector3 LocalScaling

-                {

-                        get

-                        {

-                                return _localScaling;

-                        }

-                        set

-                        {

-                                _localScaling = value;

-                        }

-                }

-

-                public override string Name

-                {

-                        get

-                        {

-                                return "Compound";

-                        }

-                }

-

-                public override float Margin

-                {

-                        get

-                        {

-                                return _collisionMargin;

-                        }

-                        set

-                        {

-                                _collisionMargin = value;

-                        }

-                }

-

-                public int ChildShapeCount { get { return _childShapes.Count; } }

-                //this is optional, but should make collision queries faster, by culling non-overlapping nodes

-                public OptimizedBvh AabbTree { get { return _aabbTree; } }

-

-                public CollisionShape GetChildShape(int index)

-                {

-                        return _childShapes[index];

-                }

-

-                public Matrix GetChildTransform(int index)

-                {

-                        return _childTransforms[index];

-                }

-

-                public override void CalculateLocalInertia(float mass, out Vector3 inertia)

-                {

-                        //approximation: take the inertia from the aabb for now

-                        Matrix ident = Matrix.Identity;

-                        Vector3 aabbMin, aabbMax;

-                        GetAabb(ident, out aabbMin, out aabbMax);

-

-                        Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f;

-

-                        float lx = 2f * (halfExtents.X);

-                        float ly = 2f * (halfExtents.Y);

-                        float lz = 2f * (halfExtents.Z);

-

-                        inertia = new Vector3();

-                        inertia.X = mass / (12.0f) * (ly * ly + lz * lz);

-                        inertia.Y = mass / (12.0f) * (lx * lx + lz * lz);

-                        inertia.Z = mass / (12.0f) * (lx * lx + ly * ly);

-                }

-

-                public void AddChildShape(Matrix localTransform, CollisionShape shape)

-                {

-                        _childTransforms.Add(localTransform);

-                        _childShapes.Add(shape);

-

-                        //extend the local aabbMin/aabbMax

-                        Vector3 localAabbMin, localAabbMax;

-                        shape.GetAabb(localTransform, out localAabbMin, out localAabbMax);

-                        if (_localAabbMin.X > localAabbMin.X)

-                        {

-                                _localAabbMin.X = localAabbMin.X;

-                        }

-                        if (_localAabbMax.X < localAabbMax.X)

-                        {

-                                _localAabbMax.X = localAabbMax.X;

-                        }

-                        if (_localAabbMin.Y > localAabbMin.Y)

-                        {

-                                _localAabbMin.Y = localAabbMin.Y;

-                        }

-                        if (_localAabbMax.Y < localAabbMax.Y)

-                        {

-                                _localAabbMax.Y = localAabbMax.Y;

-                        }

-                        if (_localAabbMin.Z > localAabbMin.Z)

-                        {

-                                _localAabbMin.Z = localAabbMin.Z;

-                        }

-                        if (_localAabbMax.Z < localAabbMax.Z)

-                        {

-                                _localAabbMax.Z = localAabbMax.Z;

-                        }

-                }

-        }

-}

+/*
+  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>
+        /// CompoundShape allows to store multiple other CollisionShapes
+        /// This allows for concave collision objects. This is more general then the Static Concave TriangleMeshShape.
+        /// </summary>
+        public class CompoundShape : CollisionShape
+        {
+                private List<Matrix> _childTransforms = new List<Matrix>();
+                private List<CollisionShape> _childShapes = new List<CollisionShape>();
+                private Vector3 _localAabbMin;
+                private Vector3 _localAabbMax;
+
+                private OptimizedBvh _aabbTree;
+                private float _collisionMargin;
+                private Vector3 _localScaling;
+
+                public CompoundShape()
+                {
+                        _localAabbMin = new Vector3(1e30f, 1e30f, 1e30f);
+                        _localAabbMax = new Vector3(-1e30f, -1e30f, -1e30f);
+                        _aabbTree = null;
+                        _collisionMargin = 0f;
+                        _localScaling = new Vector3(1f, 1f, 1f);
+                }
+
+                public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
+                {
+                        Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin);
+                        Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin);
+
+                        Matrix abs_b = MathHelper.Absolute(t);
+
+                        Vector3 row1 = new Vector3(abs_b.M11, abs_b.M12, abs_b.M13);
+                        Vector3 row2 = new Vector3(abs_b.M21, abs_b.M22, abs_b.M23);
+                        Vector3 row3 = new Vector3(abs_b.M31, abs_b.M32, abs_b.M33);
+
+                        Vector3 center = new Vector3(Vector3.Dot(row1, localCenter) + t.Translation.X,
+                                                                                 Vector3.Dot(row2, localCenter) + t.Translation.Y,
+                                                                                 Vector3.Dot(row3, localCenter) + t.Translation.Z);
+
+                        Vector3 extent = new Vector3(Vector3.Dot(row1, localHalfExtents),
+                                                                                 Vector3.Dot(row2, localHalfExtents),
+                                                                                 Vector3.Dot(row3, localHalfExtents));
+
+                        aabbMin = center - extent;
+                        aabbMax = center + extent;
+                }
+
+                public override BroadphaseNativeTypes ShapeType
+                {
+                        get
+                        {
+                                return BroadphaseNativeTypes.Compound;
+                        }
+                }
+
+                public override Vector3 LocalScaling
+                {
+                        get
+                        {
+                                return _localScaling;
+                        }
+                        set
+                        {
+                                _localScaling = value;
+                        }
+                }
+
+                public override string Name
+                {
+                        get
+                        {
+                                return "Compound";
+                        }
+                }
+
+                public override float Margin
+                {
+                        get
+                        {
+                                return _collisionMargin;
+                        }
+                        set
+                        {
+                                _collisionMargin = value;
+                        }
+                }
+
+                public int ChildShapeCount { get { return _childShapes.Count; } }
+                //this is optional, but should make collision queries faster, by culling non-overlapping nodes
+                public OptimizedBvh AabbTree { get { return _aabbTree; } }
+
+                public CollisionShape GetChildShape(int index)
+                {
+                        return _childShapes[index];
+                }
+
+                public Matrix GetChildTransform(int index)
+                {
+                        return _childTransforms[index];
+                }
+
+                public override void CalculateLocalInertia(float mass, out Vector3 inertia)
+                {
+                        //approximation: take the inertia from the aabb for now
+                        Matrix ident = Matrix.Identity;
+                        Vector3 aabbMin, aabbMax;
+                        GetAabb(ident, out aabbMin, out aabbMax);
+
+                        Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f;
+
+                        float lx = 2f * (halfExtents.X);
+                        float ly = 2f * (halfExtents.Y);
+                        float lz = 2f * (halfExtents.Z);
+
+                        inertia = new Vector3();
+                        inertia.X = mass / (12.0f) * (ly * ly + lz * lz);
+                        inertia.Y = mass / (12.0f) * (lx * lx + lz * lz);
+                        inertia.Z = mass / (12.0f) * (lx * lx + ly * ly);
+                }
+
+                public void AddChildShape(Matrix localTransform, CollisionShape shape)
+                {
+                        _childTransforms.Add(localTransform);
+                        _childShapes.Add(shape);
+
+                        //extend the local aabbMin/aabbMax
+                        Vector3 localAabbMin, localAabbMax;
+                        shape.GetAabb(localTransform, out localAabbMin, out localAabbMax);
+                        if (_localAabbMin.X > localAabbMin.X)
+                        {
+                                _localAabbMin.X = localAabbMin.X;
+                        }
+                        if (_localAabbMax.X < localAabbMax.X)
+                        {
+                                _localAabbMax.X = localAabbMax.X;
+                        }
+                        if (_localAabbMin.Y > localAabbMin.Y)
+                        {
+                                _localAabbMin.Y = localAabbMin.Y;
+                        }
+                        if (_localAabbMax.Y < localAabbMax.Y)
+                        {
+                                _localAabbMax.Y = localAabbMax.Y;
+                        }
+                        if (_localAabbMin.Z > localAabbMin.Z)
+                        {
+                                _localAabbMin.Z = localAabbMin.Z;
+                        }
+                        if (_localAabbMax.Z < localAabbMax.Z)
+                        {
+                                _localAabbMax.Z = localAabbMax.Z;
+                        }
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs
index 1e773f0..c0b177d 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs
@@ -1,55 +1,55 @@
-/*

-  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 ConcaveShape : CollisionShape

-        {

-                private float _collisionMargin;

-

-                public ConcaveShape() { }

-

-                public float CollisionMargin

-                {

-                        get { return _collisionMargin; }

-                        set { _collisionMargin = value; }

-                }

-

-                public override float Margin

-                {

-                        get

-                        {

-                                return _collisionMargin;

-                        }

-                        set

-                        {

-                                _collisionMargin = value;

-                        }

-                }

-

-                public abstract void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax);

-        }

+/*
+  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 ConcaveShape : CollisionShape
+        {
+                private float _collisionMargin;
+
+                public ConcaveShape() { }
+
+                public float CollisionMargin
+                {
+                        get { return _collisionMargin; }
+                        set { _collisionMargin = value; }
+                }
+
+                public override float Margin
+                {
+                        get
+                        {
+                                return _collisionMargin;
+                        }
+                        set
+                        {
+                                _collisionMargin = value;
+                        }
+                }
+
+                public abstract void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax);
+        }
 }
\ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs
index 361c277..52084f4 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs
@@ -1,208 +1,208 @@
-/*

-  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>

-        /// ConeShape implements a Cone shape, around the X axis

-        /// </summary>

-        public class ConeShapeX : ConeShape

-        {

-                public ConeShapeX(float radius, float height)

-                        : base(radius, height)

-                {

-                        ConeUpIndex = 0;

-                }

-        }

-

-        /// <summary>

-        /// ConeShape implements a Cone shape, around the Z axis

-        /// </summary>

-        public class ConeShapeZ : ConeShape

-        {

-                public ConeShapeZ(float radius, float height)

-                        : base(radius, height)

-                {

-                        ConeUpIndex = 2;

-                }

-        }

-

-        /// <summary>

-        /// ConeShape implements a Cone shape, around the Y axis

-        /// </summary>

-        public class ConeShape : ConvexShape

-        {

-                private float _sinAngle;

-                private float _radius;

-                private float _height;

-                private int[] _coneIndices = new int[3];

-

-                public ConeShape(float radius, float height)

-                {

-                        _radius = radius;

-                        _height = height;

-                        ConeUpIndex = 1;

-                        _sinAngle = (_radius / (float)Math.Sqrt(_radius * _radius + _height * _height));

-                }

-

-                public float Radius { get { return _radius; } }

-                public float Height { get { return _height; } }

-

-                public override BroadphaseNativeTypes ShapeType

-                {

-                        get

-                        {

-                                return BroadphaseNativeTypes.Cone;

-                        }

-                }

-

-                public override string Name

-                {

-                        get

-                        {

-                                return "Cone";

-                        }

-                }

-

-                //choose upAxis index

-                public int ConeUpIndex

-                {

-                        get { return _coneIndices[1]; }

-                        set

-                        {

-                                switch (value)

-                                {

-                                        case 0:

-                                                _coneIndices[0] = 1;

-                                                _coneIndices[1] = 0;

-                                                _coneIndices[2] = 2;

-                                                break;

-                                        case 1:

-                                                _coneIndices[0] = 0;

-                                                _coneIndices[1] = 1;

-                                                _coneIndices[2] = 2;

-                                                break;

-                                        case 2:

-                                                _coneIndices[0] = 0;

-                                                _coneIndices[1] = 2;

-                                                _coneIndices[2] = 1;

-                                                break;

-                                        default:

-                                                BulletDebug.Assert(false);

-                                                break;

-                                }

-                        }

-                }

-

-                private Vector3 ConeLocalSupport(Vector3 v)

-                {

-                        float halfHeight = _height * 0.5f;

-                        bool condition;

-

-                        if (_coneIndices[1] == 0)

-                                condition = v.X > v.Length() * _sinAngle;

-                        else if (_coneIndices[1] == 1)

-                                condition = v.Y > v.Length() * _sinAngle;

-                        else

-                                condition = v.Z > v.Length() * _sinAngle;

-

-                        if (condition)

-                        {

-                                Vector3 tmp = new Vector3();

-                                MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], halfHeight);

-                                return tmp;

-                        }

-                        else

-                        {

-                                float s = (float)Math.Sqrt(MathHelper.GetValueByIndex(v, _coneIndices[0]) * MathHelper.GetValueByIndex(v, _coneIndices[0])

-                                        + MathHelper.GetValueByIndex(v, _coneIndices[2]) * MathHelper.GetValueByIndex(v, _coneIndices[2]));

-                                if (s > MathHelper.Epsilon)

-                                {

-                                        float d = _radius / s;

-                                        Vector3 tmp = new Vector3();

-                                        MathHelper.SetValueByIndex(ref tmp, _coneIndices[0], MathHelper.GetValueByIndex(v, _coneIndices[0]) * d);

-                                        MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight);

-                                        MathHelper.SetValueByIndex(ref tmp, _coneIndices[2], MathHelper.GetValueByIndex(v, _coneIndices[2]) * d);

-                                        return tmp;

-                                }

-                                else

-                                {

-                                        Vector3 tmp = new Vector3();

-                                        MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight);

-                                        return tmp;

-                                }

-                        }

-                }

-

-                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)

-                {

-                        return ConeLocalSupport(vec);

-                }

-

-                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)

-                {

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

-                                supportVerticesOut[i] = ConeLocalSupport(vectors[i]);

-                }

-

-                public override void CalculateLocalInertia(float mass, out Vector3 inertia)

-                {

-                        Matrix identity = Matrix.Identity;

-                        Vector3 aabbMin, aabbMax;

-                        GetAabb(identity, out aabbMin, out aabbMax);

-

-                        Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f;

-

-                        float margin = Margin;

-

-                        float lx = 2f * (halfExtents.X + margin);

-                        float ly = 2f * (halfExtents.Y + margin);

-                        float lz = 2f * (halfExtents.Z + margin);

-                        float x2 = lx * lx;

-                        float y2 = ly * ly;

-                        float z2 = lz * lz;

-                        float scaledmass = mass * 0.08333333f;

-

-                        inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2));

-                }

-

-                public override Vector3 LocalGetSupportingVertex(Vector3 vec)

-                {

-                        Vector3 supVertex = ConeLocalSupport(vec);

-                        if (Margin != 0)

-                        {

-                                Vector3 vecnorm = vec;

-                                if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))

-                                {

-                                        vecnorm = new Vector3(-1f, -1f, -1f);

-                                }

-                                vecnorm = Vector3.Normalize(vecnorm);

-                                supVertex += Margin * vecnorm;

-                        }

-                        return supVertex;

-                }

-        }

-}

+/*
+  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>
+        /// ConeShape implements a Cone shape, around the X axis
+        /// </summary>
+        public class ConeShapeX : ConeShape
+        {
+                public ConeShapeX(float radius, float height)
+                        : base(radius, height)
+                {
+                        ConeUpIndex = 0;
+                }
+        }
+
+        /// <summary>
+        /// ConeShape implements a Cone shape, around the Z axis
+        /// </summary>
+        public class ConeShapeZ : ConeShape
+        {
+                public ConeShapeZ(float radius, float height)
+                        : base(radius, height)
+                {
+                        ConeUpIndex = 2;
+                }
+        }
+
+        /// <summary>
+        /// ConeShape implements a Cone shape, around the Y axis
+        /// </summary>
+        public class ConeShape : ConvexShape
+        {
+                private float _sinAngle;
+                private float _radius;
+                private float _height;
+                private int[] _coneIndices = new int[3];
+
+                public ConeShape(float radius, float height)
+                {
+                        _radius = radius;
+                        _height = height;
+                        ConeUpIndex = 1;
+                        _sinAngle = (_radius / (float)Math.Sqrt(_radius * _radius + _height * _height));
+                }
+
+                public float Radius { get { return _radius; } }
+                public float Height { get { return _height; } }
+
+                public override BroadphaseNativeTypes ShapeType
+                {
+                        get
+                        {
+                                return BroadphaseNativeTypes.Cone;
+                        }
+                }
+
+                public override string Name
+                {
+                        get
+                        {
+                                return "Cone";
+                        }
+                }
+
+                //choose upAxis index
+                public int ConeUpIndex
+                {
+                        get { return _coneIndices[1]; }
+                        set
+                        {
+                                switch (value)
+                                {
+                                        case 0:
+                                                _coneIndices[0] = 1;
+                                                _coneIndices[1] = 0;
+                                                _coneIndices[2] = 2;
+                                                break;
+                                        case 1:
+                                                _coneIndices[0] = 0;
+                                                _coneIndices[1] = 1;
+                                                _coneIndices[2] = 2;
+                                                break;
+                                        case 2:
+                                                _coneIndices[0] = 0;
+                                                _coneIndices[1] = 2;
+                                                _coneIndices[2] = 1;
+                                                break;
+                                        default:
+                                                BulletDebug.Assert(false);
+                                                break;
+                                }
+                        }
+                }
+
+                private Vector3 ConeLocalSupport(Vector3 v)
+                {
+                        float halfHeight = _height * 0.5f;
+                        bool condition;
+
+                        if (_coneIndices[1] == 0)
+                                condition = v.X > v.Length() * _sinAngle;
+                        else if (_coneIndices[1] == 1)
+                                condition = v.Y > v.Length() * _sinAngle;
+                        else
+                                condition = v.Z > v.Length() * _sinAngle;
+
+                        if (condition)
+                        {
+                                Vector3 tmp = new Vector3();
+                                MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], halfHeight);
+                                return tmp;
+                        }
+                        else
+                        {
+                                float s = (float)Math.Sqrt(MathHelper.GetValueByIndex(v, _coneIndices[0]) * MathHelper.GetValueByIndex(v, _coneIndices[0])
+                                        + MathHelper.GetValueByIndex(v, _coneIndices[2]) * MathHelper.GetValueByIndex(v, _coneIndices[2]));
+                                if (s > MathHelper.Epsilon)
+                                {
+                                        float d = _radius / s;
+                                        Vector3 tmp = new Vector3();
+                                        MathHelper.SetValueByIndex(ref tmp, _coneIndices[0], MathHelper.GetValueByIndex(v, _coneIndices[0]) * d);
+                                        MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight);
+                                        MathHelper.SetValueByIndex(ref tmp, _coneIndices[2], MathHelper.GetValueByIndex(v, _coneIndices[2]) * d);
+                                        return tmp;
+                                }
+                                else
+                                {
+                                        Vector3 tmp = new Vector3();
+                                        MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight);
+                                        return tmp;
+                                }
+                        }
+                }
+
+                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
+                {
+                        return ConeLocalSupport(vec);
+                }
+
+                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
+                {
+                        for (int i = 0; i < vectors.Length; i++)
+                                supportVerticesOut[i] = ConeLocalSupport(vectors[i]);
+                }
+
+                public override void CalculateLocalInertia(float mass, out Vector3 inertia)
+                {
+                        Matrix identity = Matrix.Identity;
+                        Vector3 aabbMin, aabbMax;
+                        GetAabb(identity, out aabbMin, out aabbMax);
+
+                        Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f;
+
+                        float margin = Margin;
+
+                        float lx = 2f * (halfExtents.X + margin);
+                        float ly = 2f * (halfExtents.Y + margin);
+                        float lz = 2f * (halfExtents.Z + margin);
+                        float x2 = lx * lx;
+                        float y2 = ly * ly;
+                        float z2 = lz * lz;
+                        float scaledmass = mass * 0.08333333f;
+
+                        inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2));
+                }
+
+                public override Vector3 LocalGetSupportingVertex(Vector3 vec)
+                {
+                        Vector3 supVertex = ConeLocalSupport(vec);
+                        if (Margin != 0)
+                        {
+                                Vector3 vecnorm = vec;
+                                if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))
+                                {
+                                        vecnorm = new Vector3(-1f, -1f, -1f);
+                                }
+                                vecnorm = Vector3.Normalize(vecnorm);
+                                supVertex += Margin * vecnorm;
+                        }
+                        return supVertex;
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs
index eb78533..8d06c90 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs
@@ -1,184 +1,184 @@
-/*

-  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>

-        /// ConvexHullShape implements an implicit (getSupportingVertex) Convex Hull of a Point Cloud (vertices)

-        /// No connectivity is needed. localGetSupportingVertex iterates linearly though all vertices.

-        /// on modern hardware, due to cache coherency this isn't that bad. Complex algorithms tend to trash the cash.

-        /// (memory is much slower then the cpu)

-        /// </summary>

-        public class ConvexHullShape : PolyhedralConvexShape

-        {

-                private List<Vector3> _points = new List<Vector3>();

-

-                public ConvexHullShape() { }

-

-                public override int VertexCount

-                {

-                        get

-                        {

-                                return _points.Count;

-                        }

-                }

-

-                public override int EdgeCount

-                {

-                        get

-                        {

-                                return _points.Count;

-                        }

-                }

-

-                public override int PlaneCount

-                {

-                        get

-                        {

-                                return 0;

-                        }

-                }

-

-                public override BroadphaseNativeTypes ShapeType

-                {

-                        get

-                        {

-                                return BroadphaseNativeTypes.ConvexHull;

-                        }

-                }

-

-                public override string Name

-                {

-                        get

-                        {

-                                return "Convex";

-                        }

-                }

-

-                public override Vector3 LocalGetSupportingVertex(Vector3 vec)

-                {

-                        Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec);

-

-                        if (Margin != 0)

-                        {

-                                Vector3 vecnorm = vec;

-                                if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))

-                                {

-                                        vecnorm=new Vector3(-1, -1, -1);

-                                }

-                                vecnorm = Vector3.Normalize(vecnorm);

-                                supVertex += Margin * vecnorm;

-                        }

-                        return supVertex;

-                }

-

-                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0)

-                {

-                        Vector3 supVec = new Vector3();

-                        float newDot, maxDot = -1e30f;

-

-                        Vector3 vec = vec0;

-                        float lenSqr = vec.LengthSquared();

-                        if (lenSqr < 0.0001f)

-                        {

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

-                        }

-                        else

-                        {

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

-                                vec *= rlen;

-                        }

-

-                        for (int i = 0; i < _points.Count; i++)

-                        {

-                                Vector3 vtx = _points[i] * LocalScaling;

-

-                                newDot = Vector3.Dot(vec, vtx);

-                                if (newDot > maxDot)

-                                {

-                                        maxDot = newDot;

-                                        supVec = vtx;

-                                }

-                        }

-                        return supVec;

-                }

-

-                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)

-                {

-                        float newDot;

-                        //use 'w' component of supportVerticesOut?

-                        /*{

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

-                                {

-                                        supportVerticesOut[i][3] = -1e30f;

-                                }

-                        }*/

-                        #warning Warning!

-                        for (int i = 0; i < _points.Count; i++)

-                        {

-                                Vector3 vtx = _points[i] * LocalScaling;

-

-                                for (int j = 0; j < vectors.Length; j++)

-                                {

-                                        newDot = Vector3.Dot(vectors[j], vtx);

-                                        if (newDot > -1e30f)

-                                        {

-                                                //WARNING: don't swap next lines, the w component would get overwritten!

-                                                supportVerticesOut[j] = vtx;

-                                                //supportVerticesOut[j][3] = newDot;

-                                                #warning Warning!

-                                        }

-                                }

-                        }

-                }

-

-                public override void GetEdge(int i, out Vector3 pa, out Vector3 pb)

-                {

-                        int index0 = i % _points.Count;

-                        int index1 = (i + 1) % _points.Count;

-                        pa = _points[index0] * LocalScaling;

-                        pb = _points[index1] * LocalScaling;

-                }

-

-                public override void GetVertex(int i, out Vector3 vtx)

-                {

-                        vtx = _points[i] * LocalScaling;

-                }

-

-                public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i)

-                {

-                        planeNormal = new Vector3();

-                        planeSupport = new Vector3();

-                        BulletDebug.Assert(false);

-                }

-

-                public override bool IsInside(Vector3 pt, float tolerance)

-                {

-                        BulletDebug.Assert(false);

-                        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>
+        /// ConvexHullShape implements an implicit (getSupportingVertex) Convex Hull of a Point Cloud (vertices)
+        /// No connectivity is needed. localGetSupportingVertex iterates linearly though all vertices.
+        /// on modern hardware, due to cache coherency this isn't that bad. Complex algorithms tend to trash the cash.
+        /// (memory is much slower then the cpu)
+        /// </summary>
+        public class ConvexHullShape : PolyhedralConvexShape
+        {
+                private List<Vector3> _points = new List<Vector3>();
+
+                public ConvexHullShape() { }
+
+                public override int VertexCount
+                {
+                        get
+                        {
+                                return _points.Count;
+                        }
+                }
+
+                public override int EdgeCount
+                {
+                        get
+                        {
+                                return _points.Count;
+                        }
+                }
+
+                public override int PlaneCount
+                {
+                        get
+                        {
+                                return 0;
+                        }
+                }
+
+                public override BroadphaseNativeTypes ShapeType
+                {
+                        get
+                        {
+                                return BroadphaseNativeTypes.ConvexHull;
+                        }
+                }
+
+                public override string Name
+                {
+                        get
+                        {
+                                return "Convex";
+                        }
+                }
+
+                public override Vector3 LocalGetSupportingVertex(Vector3 vec)
+                {
+                        Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec);
+
+                        if (Margin != 0)
+                        {
+                                Vector3 vecnorm = vec;
+                                if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))
+                                {
+                                        vecnorm=new Vector3(-1, -1, -1);
+                                }
+                                vecnorm = Vector3.Normalize(vecnorm);
+                                supVertex += Margin * vecnorm;
+                        }
+                        return supVertex;
+                }
+
+                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0)
+                {
+                        Vector3 supVec = new Vector3();
+                        float newDot, maxDot = -1e30f;
+
+                        Vector3 vec = vec0;
+                        float lenSqr = vec.LengthSquared();
+                        if (lenSqr < 0.0001f)
+                        {
+                                vec = new Vector3(1, 0, 0);
+                        }
+                        else
+                        {
+                                float rlen = 1f / (float)Math.Sqrt(lenSqr);
+                                vec *= rlen;
+                        }
+
+                        for (int i = 0; i < _points.Count; i++)
+                        {
+                                Vector3 vtx = _points[i] * LocalScaling;
+
+                                newDot = Vector3.Dot(vec, vtx);
+                                if (newDot > maxDot)
+                                {
+                                        maxDot = newDot;
+                                        supVec = vtx;
+                                }
+                        }
+                        return supVec;
+                }
+
+                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
+                {
+                        float newDot;
+                        //use 'w' component of supportVerticesOut?
+                        /*{
+                                for (int i = 0; i < numVectors; i++)
+                                {
+                                        supportVerticesOut[i][3] = -1e30f;
+                                }
+                        }*/
+                        #warning Warning!
+                        for (int i = 0; i < _points.Count; i++)
+                        {
+                                Vector3 vtx = _points[i] * LocalScaling;
+
+                                for (int j = 0; j < vectors.Length; j++)
+                                {
+                                        newDot = Vector3.Dot(vectors[j], vtx);
+                                        if (newDot > -1e30f)
+                                        {
+                                                //WARNING: don't swap next lines, the w component would get overwritten!
+                                                supportVerticesOut[j] = vtx;
+                                                //supportVerticesOut[j][3] = newDot;
+                                                #warning Warning!
+                                        }
+                                }
+                        }
+                }
+
+                public override void GetEdge(int i, out Vector3 pa, out Vector3 pb)
+                {
+                        int index0 = i % _points.Count;
+                        int index1 = (i + 1) % _points.Count;
+                        pa = _points[index0] * LocalScaling;
+                        pb = _points[index1] * LocalScaling;
+                }
+
+                public override void GetVertex(int i, out Vector3 vtx)
+                {
+                        vtx = _points[i] * LocalScaling;
+                }
+
+                public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i)
+                {
+                        planeNormal = new Vector3();
+                        planeSupport = new Vector3();
+                        BulletDebug.Assert(false);
+                }
+
+                public override bool IsInside(Vector3 pt, float tolerance)
+                {
+                        BulletDebug.Assert(false);
+                        return false;
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs
index 02d678e..d5c2fbd 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs
@@ -1,141 +1,141 @@
-/*

-  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>

-        /// ConvexShape is an abstract shape interface.

-        /// The explicit part provides plane-equations, the implicit part provides GetClosestPoint interface.

-        /// used in combination with GJK or btConvexCast

-        /// </summary>

-        public abstract class ConvexShape : CollisionShape

-        {

-                private const int _maxPreferredPenetrationDirections = 10;

-                private const float _convexDistanceMargin = 0.04f;

-

-                private Vector3 _localScaling;

-                private Vector3 _implicitShapeDimensions;

-                private float _collisionMargin;

-

-                public ConvexShape()

-                        : base()

-                {

-                        _localScaling = Vector3.One;

-                        _collisionMargin = ConvexDistanceMargin;

-                }

-

-                public static int MaxPreferredPenetrationDirections { get { return _maxPreferredPenetrationDirections; } }

-                public static float ConvexDistanceMargin { get { return _convexDistanceMargin; } }

-

-                public Vector3 ImplicitShapeDimensions { get { return _implicitShapeDimensions; } protected set { _implicitShapeDimensions = value; } }

-                public virtual int PreferredPenetrationDirectionsCount { get { return 0; } }

-

-                protected float CollisionMargin { get { return _collisionMargin; } set { _collisionMargin = value; } }

-

-                public virtual void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector)

-                {

-                        penetrationVector = new Vector3();

-                        BulletDebug.Assert(false);

-                }

-

-                public abstract Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec);

-                //notice that the vectors should be unit length

-                public abstract void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut);

-

-                /// <summary>

-                /// getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version

-                /// </summary>

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

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

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

-                public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)

-                {

-                        GetAabbSlow(t, out aabbMin, out aabbMax);

-                }

-

-                public override Vector3 LocalScaling

-                {

-                        get

-                        {

-                                return _localScaling;

-                        }

-                        set

-                        {

-                                _localScaling = value;

-                        }

-                }

-

-                public override float Margin

-                {

-                        get

-                        {

-                                return _collisionMargin;

-                        }

-                        set

-                        {

-                                _collisionMargin = value;

-                        }

-                }

-

-                public virtual Vector3 LocalGetSupportingVertex(Vector3 vec)

-                {

-                        Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec);

-

-                        if (Margin != 0f)

-                        {

-                                Vector3 vecnorm = vec;

-                                if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))

-                                {

-                                        vecnorm = new Vector3(-1f, -1f, -1f);

-                                }

-                                vecnorm.Normalize();

-                                supVertex += Margin * vecnorm;

-                        }

-                        return supVertex;

-                }

-

-                public virtual void GetAabbSlow(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)

-                {

-                        float margin = Margin;

-                        aabbMax = new Vector3();

-                        aabbMin = new Vector3();

-

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

-                        {

-                                Vector3 vec = new Vector3(0f, 0f, 0f);

-                                MathHelper.SetElement(ref vec, i, 1);

-

-                                Vector3 sv = LocalGetSupportingVertex(Vector3.TransformNormal(vec, t));

-

-                                Vector3 tmp = MathHelper.MatrixToVector(t, sv);

-                                MathHelper.SetElement(ref aabbMax, i, MathHelper.GetElement(tmp, i) + margin);

-                                MathHelper.SetElement(ref vec, i, -1f);

-                                tmp = MathHelper.MatrixToVector(t, LocalGetSupportingVertex(Vector3.TransformNormal(vec, t)));

-                                MathHelper.SetElement(ref aabbMin, i, MathHelper.GetElement(tmp, i) - margin);

-                        }

-                }

-        }

-}

+/*
+  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>
+        /// ConvexShape is an abstract shape interface.
+        /// The explicit part provides plane-equations, the implicit part provides GetClosestPoint interface.
+        /// used in combination with GJK or btConvexCast
+        /// </summary>
+        public abstract class ConvexShape : CollisionShape
+        {
+                private const int _maxPreferredPenetrationDirections = 10;
+                private const float _convexDistanceMargin = 0.04f;
+
+                private Vector3 _localScaling;
+                private Vector3 _implicitShapeDimensions;
+                private float _collisionMargin;
+
+                public ConvexShape()
+                        : base()
+                {
+                        _localScaling = Vector3.One;
+                        _collisionMargin = ConvexDistanceMargin;
+                }
+
+                public static int MaxPreferredPenetrationDirections { get { return _maxPreferredPenetrationDirections; } }
+                public static float ConvexDistanceMargin { get { return _convexDistanceMargin; } }
+
+                public Vector3 ImplicitShapeDimensions { get { return _implicitShapeDimensions; } protected set { _implicitShapeDimensions = value; } }
+                public virtual int PreferredPenetrationDirectionsCount { get { return 0; } }
+
+                protected float CollisionMargin { get { return _collisionMargin; } set { _collisionMargin = value; } }
+
+                public virtual void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector)
+                {
+                        penetrationVector = new Vector3();
+                        BulletDebug.Assert(false);
+                }
+
+                public abstract Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec);
+                //notice that the vectors should be unit length
+                public abstract void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut);
+
+                /// <summary>
+                /// getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+                /// </summary>
+                /// <param name="t"></param>
+                /// <param name="aabbMin"></param>
+                /// <param name="aabbMax"></param>
+                public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
+                {
+                        GetAabbSlow(t, out aabbMin, out aabbMax);
+                }
+
+                public override Vector3 LocalScaling
+                {
+                        get
+                        {
+                                return _localScaling;
+                        }
+                        set
+                        {
+                                _localScaling = value;
+                        }
+                }
+
+                public override float Margin
+                {
+                        get
+                        {
+                                return _collisionMargin;
+                        }
+                        set
+                        {
+                                _collisionMargin = value;
+                        }
+                }
+
+                public virtual Vector3 LocalGetSupportingVertex(Vector3 vec)
+                {
+                        Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec);
+
+                        if (Margin != 0f)
+                        {
+                                Vector3 vecnorm = vec;
+                                if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))
+                                {
+                                        vecnorm = new Vector3(-1f, -1f, -1f);
+                                }
+                                vecnorm.Normalize();
+                                supVertex += Margin * vecnorm;
+                        }
+                        return supVertex;
+                }
+
+                public virtual void GetAabbSlow(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
+                {
+                        float margin = Margin;
+                        aabbMax = new Vector3();
+                        aabbMin = new Vector3();
+
+                        for (int i = 0; i < 3; i++)
+                        {
+                                Vector3 vec = new Vector3(0f, 0f, 0f);
+                                MathHelper.SetElement(ref vec, i, 1);
+
+                                Vector3 sv = LocalGetSupportingVertex(Vector3.TransformNormal(vec, t));
+
+                                Vector3 tmp = MathHelper.MatrixToVector(t, sv);
+                                MathHelper.SetElement(ref aabbMax, i, MathHelper.GetElement(tmp, i) + margin);
+                                MathHelper.SetElement(ref vec, i, -1f);
+                                tmp = MathHelper.MatrixToVector(t, LocalGetSupportingVertex(Vector3.TransformNormal(vec, t)));
+                                MathHelper.SetElement(ref aabbMin, i, MathHelper.GetElement(tmp, i) - margin);
+                        }
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs
index 0fd3fa7..3e46c66 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs
@@ -1,185 +1,185 @@
-/*

-  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>

-        /// ConvexTriangleMeshShape is a convex hull of a triangle mesh. If you just have a point cloud, you can use ConvexHullShape instead.

-        /// It uses the StridingMeshInterface instead of a point cloud. This can avoid the duplication of the triangle mesh data.

-        /// </summary>

-        public class ConvexTriangleMeshShape : PolyhedralConvexShape

-        {

-                private StridingMeshInterface _stridingMesh;

-

-                public ConvexTriangleMeshShape(StridingMeshInterface meshInterface)

-                {

-                        _stridingMesh = meshInterface;

-                }

-

-                public StridingMeshInterface getStridingMesh()

-                {

-                        return _stridingMesh;

-                }

-

-                public override int VertexCount

-                {

-                        get

-                        {

-                                return 0;

-                        }

-                }

-

-                public override int EdgeCount

-                {

-                        get

-                        {

-                                return 0;

-                        }

-                }

-

-                public override int PlaneCount

-                {

-                        get

-                        {

-                                return 0;

-                        }

-                }

-

-                public override Vector3 LocalScaling

-                {

-                        get

-                        {

-                                return base.LocalScaling;

-                        }

-                        set

-                        {

-                                _stridingMesh.Scaling = value;

-                        }

-                }

-

-                public override BroadphaseNativeTypes ShapeType

-                {

-                        get

-                        {

-                                return BroadphaseNativeTypes.ConvexTriangleMesh;

-                        }

-                }

-

-                public override string Name

-                {

-                        get

-                        {

-                                return "ConvexTrimesh";

-                        }

-                }

-

-                public override void GetEdge(int i, out Vector3 pa, out Vector3 pb)

-                {

-                        pa = new Vector3();

-                        pb = new Vector3();

-                        BulletDebug.Assert(false);

-                }

-

-                public override void GetVertex(int i, out Vector3 vtx)

-                {

-                        vtx = new Vector3();

-                        BulletDebug.Assert(false);

-                }

-

-                public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i)

-                {

-                        planeNormal = new Vector3();

-                        planeSupport = new Vector3();

-                        BulletDebug.Assert(false);

-                }

-

-                public override bool IsInside(Vector3 pt, float tolerance)

-                {

-                        BulletDebug.Assert(false);

-                        return false;

-                }

-

-                public override Vector3 LocalGetSupportingVertex(Vector3 vec)

-                {

-                        Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec);

-

-                        if (Margin != 0)

-                        {

-                                Vector3 vecnorm = vec;

-                                if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))

-                                {

-                                        vecnorm = new Vector3(-1, -1, -1);

-                                }

-                                vecnorm = Vector3.Normalize(vecnorm);

-                                supVertex += Margin * vecnorm;

-                        }

-                        return supVertex;

-                }

-

-                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0)

-                {

-                        Vector3 supVec = new Vector3();

-

-                        Vector3 vec = vec0;

-                        float lenSqr = vec.LengthSquared();

-                        if (lenSqr < 0.0001f)

-                        {

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

-                        }

-                        else

-                        {

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

-                                vec *= rlen;

-                        }

-

-                        LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec);

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

-                        _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax);

-                        supVec = supportCallback.SupportVertexLocal;

-

-                        return supVec;

-                }

-

-                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)

-                {

-                        //use 'w' component of supportVerticesOut?

-                        /*{

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

-                                {

-                                        supportVerticesOut[i][3] = -1e30f;

-                                }

-                        }*/

-                        for (int j = 0; j < vectors.Length; j++)

-                        {

-                                Vector3 vec = vectors[j];

-                                LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec);

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

-                                _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax);

-                                supportVerticesOut[j] = supportCallback.SupportVertexLocal;

-                        }

-                }

-        }

-}

+/*
+  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>
+        /// ConvexTriangleMeshShape is a convex hull of a triangle mesh. If you just have a point cloud, you can use ConvexHullShape instead.
+        /// It uses the StridingMeshInterface instead of a point cloud. This can avoid the duplication of the triangle mesh data.
+        /// </summary>
+        public class ConvexTriangleMeshShape : PolyhedralConvexShape
+        {
+                private StridingMeshInterface _stridingMesh;
+
+                public ConvexTriangleMeshShape(StridingMeshInterface meshInterface)
+                {
+                        _stridingMesh = meshInterface;
+                }
+
+                public StridingMeshInterface getStridingMesh()
+                {
+                        return _stridingMesh;
+                }
+
+                public override int VertexCount
+                {
+                        get
+                        {
+                                return 0;
+                        }
+                }
+
+                public override int EdgeCount
+                {
+                        get
+                        {
+                                return 0;
+                        }
+                }
+
+                public override int PlaneCount
+                {
+                        get
+                        {
+                                return 0;
+                        }
+                }
+
+                public override Vector3 LocalScaling
+                {
+                        get
+                        {
+                                return base.LocalScaling;
+                        }
+                        set
+                        {
+                                _stridingMesh.Scaling = value;
+                        }
+                }
+
+                public override BroadphaseNativeTypes ShapeType
+                {
+                        get
+                        {
+                                return BroadphaseNativeTypes.ConvexTriangleMesh;
+                        }
+                }
+
+                public override string Name
+                {
+                        get
+                        {
+                                return "ConvexTrimesh";
+                        }
+                }
+
+                public override void GetEdge(int i, out Vector3 pa, out Vector3 pb)
+                {
+                        pa = new Vector3();
+                        pb = new Vector3();
+                        BulletDebug.Assert(false);
+                }
+
+                public override void GetVertex(int i, out Vector3 vtx)
+                {
+                        vtx = new Vector3();
+                        BulletDebug.Assert(false);
+                }
+
+                public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i)
+                {
+                        planeNormal = new Vector3();
+                        planeSupport = new Vector3();
+                        BulletDebug.Assert(false);
+                }
+
+                public override bool IsInside(Vector3 pt, float tolerance)
+                {
+                        BulletDebug.Assert(false);
+                        return false;
+                }
+
+                public override Vector3 LocalGetSupportingVertex(Vector3 vec)
+                {
+                        Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec);
+
+                        if (Margin != 0)
+                        {
+                                Vector3 vecnorm = vec;
+                                if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))
+                                {
+                                        vecnorm = new Vector3(-1, -1, -1);
+                                }
+                                vecnorm = Vector3.Normalize(vecnorm);
+                                supVertex += Margin * vecnorm;
+                        }
+                        return supVertex;
+                }
+
+                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0)
+                {
+                        Vector3 supVec = new Vector3();
+
+                        Vector3 vec = vec0;
+                        float lenSqr = vec.LengthSquared();
+                        if (lenSqr < 0.0001f)
+                        {
+                                vec = new Vector3(1, 0, 0);
+                        }
+                        else
+                        {
+                                float rlen = 1f / (float)Math.Sqrt(lenSqr);
+                                vec *= rlen;
+                        }
+
+                        LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec);
+                        Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
+                        _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax);
+                        supVec = supportCallback.SupportVertexLocal;
+
+                        return supVec;
+                }
+
+                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
+                {
+                        //use 'w' component of supportVerticesOut?
+                        /*{
+                                for (int i = 0; i < numVectors; i++)
+                                {
+                                        supportVerticesOut[i][3] = -1e30f;
+                                }
+                        }*/
+                        for (int j = 0; j < vectors.Length; j++)
+                        {
+                                Vector3 vec = vectors[j];
+                                LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec);
+                                Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
+                                _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax);
+                                supportVerticesOut[j] = supportCallback.SupportVertexLocal;
+                        }
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs
index 9a86eaf..85bf769 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs
@@ -1,136 +1,136 @@
-/*

-  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>

-        /// implements cylinder shape interface

-        /// </summary>

-        public class CylinderShape : BoxShape

-        {

-                public CylinderShape(Vector3 halfExtents)

-                        : base(halfExtents)

-                {

-                }

-

-                public override BroadphaseNativeTypes ShapeType

-                {

-                        get

-                        {

-                                return BroadphaseNativeTypes.Cylinder;

-                        }

-                }

-

-                public virtual int UpAxis

-                {

-                        get

-                        {

-                                return 1;

-                        }

-                }

-

-                public virtual float Radius

-                {

-                        get

-                        {

-                                return HalfExtents.Z;

-                        }

-                }

-

-                //debugging

-                public override string Name

-                {

-                        get

-                        {

-                                return "CylinderY";

-                        }

-                }

-

-                //getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version

-                public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)

-                {

-                        GetAabbSlow(t, out aabbMin, out aabbMax);

-                }

-

-                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)

-                {

-                        return CylinderLocalSupportY(HalfExtents, vec);

-                }

-

-                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)

-                {

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

-                        {

-                                supportVerticesOut[i] = CylinderLocalSupportY(HalfExtents, vectors[i]);

-                        }

-                }

-

-                public override Vector3 LocalGetSupportingVertex(Vector3 vec)

-                {

-

-                        Vector3 supVertex;

-                        supVertex = LocalGetSupportingVertexWithoutMargin(vec);

-

-                        if (Margin != 0)

-                        {

-                                Vector3 vecnorm = vec;

-                                if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))

-                                {

-                                        vecnorm=new Vector3(-1, -1, -1);

-                                }

-                                vecnorm = Vector3.Normalize(vecnorm);

-                                supVertex += Margin * vecnorm;

-                        }

-                        return supVertex;

-                }

-

-                private Vector3 CylinderLocalSupportY(Vector3 halfExtents, Vector3 v)

-                {

-                        float radius = halfExtents.X;

-                        float halfHeight = halfExtents.Y;

-

-                        Vector3 tmp = new Vector3();

-                        float d;

-

-                        float s = (float)Math.Sqrt(v.X * v.X + v.Z * v.Z);

-                        if (s != 0)

-                        {

-                                d = radius / s;

-                                tmp.X = v.X * d;

-                                tmp.Y = v.Y < 0 ? -halfHeight : halfHeight;

-                                tmp.Z = v.Z * d;

-                                return tmp;

-                        }

-                        else

-                        {

-                                tmp.X = radius;

-                                tmp.Y = v.Y < 0 ? -halfHeight : halfHeight;

-                                tmp.Z = 0;

-                                return tmp;

-                        }

-                }

-        }

-}

+/*
+  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>
+        /// implements cylinder shape interface
+        /// </summary>
+        public class CylinderShape : BoxShape
+        {
+                public CylinderShape(Vector3 halfExtents)
+                        : base(halfExtents)
+                {
+                }
+
+                public override BroadphaseNativeTypes ShapeType
+                {
+                        get
+                        {
+                                return BroadphaseNativeTypes.Cylinder;
+                        }
+                }
+
+                public virtual int UpAxis
+                {
+                        get
+                        {
+                                return 1;
+                        }
+                }
+
+                public virtual float Radius
+                {
+                        get
+                        {
+                                return HalfExtents.Z;
+                        }
+                }
+
+                //debugging
+                public override string Name
+                {
+                        get
+                        {
+                                return "CylinderY";
+                        }
+                }
+
+                //getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
+                public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
+                {
+                        GetAabbSlow(t, out aabbMin, out aabbMax);
+                }
+
+                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
+                {
+                        return CylinderLocalSupportY(HalfExtents, vec);
+                }
+
+                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
+                {
+                        for (int i = 0; i < vectors.Length; i++)
+                        {
+                                supportVerticesOut[i] = CylinderLocalSupportY(HalfExtents, vectors[i]);
+                        }
+                }
+
+                public override Vector3 LocalGetSupportingVertex(Vector3 vec)
+                {
+
+                        Vector3 supVertex;
+                        supVertex = LocalGetSupportingVertexWithoutMargin(vec);
+
+                        if (Margin != 0)
+                        {
+                                Vector3 vecnorm = vec;
+                                if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))
+                                {
+                                        vecnorm=new Vector3(-1, -1, -1);
+                                }
+                                vecnorm = Vector3.Normalize(vecnorm);
+                                supVertex += Margin * vecnorm;
+                        }
+                        return supVertex;
+                }
+
+                private Vector3 CylinderLocalSupportY(Vector3 halfExtents, Vector3 v)
+                {
+                        float radius = halfExtents.X;
+                        float halfHeight = halfExtents.Y;
+
+                        Vector3 tmp = new Vector3();
+                        float d;
+
+                        float s = (float)Math.Sqrt(v.X * v.X + v.Z * v.Z);
+                        if (s != 0)
+                        {
+                                d = radius / s;
+                                tmp.X = v.X * d;
+                                tmp.Y = v.Y < 0 ? -halfHeight : halfHeight;
+                                tmp.Z = v.Z * d;
+                                return tmp;
+                        }
+                        else
+                        {
+                                tmp.X = radius;
+                                tmp.Y = v.Y < 0 ? -halfHeight : halfHeight;
+                                tmp.Z = 0;
+                                return tmp;
+                        }
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs
index 9dc8e2d..da84462 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs
@@ -1,100 +1,100 @@
-/*

-  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 CylinderShapeX : CylinderShape

-        {

-                public CylinderShapeX(Vector3 halfExtents)

-                        : base(halfExtents) { }

-

-                public override int UpAxis

-                {

-                        get

-                        {

-                                return 0;

-                        }

-                }

-

-                public override float Radius

-                {

-                        get

-                        {

-                                return HalfExtents.Y;

-                        }

-                }

-

-                //debugging

-                public override string Name

-                {

-                        get

-                        {

-                                return "CylinderX";

-                        }

-                }

-

-                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)

-                {

-                        return CylinderLocalSupportX(HalfExtents, vec);

-                }

-

-                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)

-                {

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

-                        {

-                                supportVerticesOut[i] = CylinderLocalSupportX(HalfExtents, vectors[i]);

-                        }

-                }

-

-                private Vector3 CylinderLocalSupportX(Vector3 halfExtents, Vector3 v)

-                {

-                        //mapping depends on how cylinder local orientation is

-                        // extents of the cylinder is: X,Y is for radius, and Z for height

-                        float radius = halfExtents.Y;

-                        float halfHeight = halfExtents.X;

-

-                        Vector3 tmp = new Vector3();

-                        float d;

-

-                        float s = (float)Math.Sqrt(v.Y * v.Y + v.Z * v.Z);

-                        if (s != 0)

-                        {

-                                d = radius / s;

-                                tmp.Y = v.Y * d;

-                                tmp.X = v.X < 0 ? -halfHeight : halfHeight;

-                                tmp.Z = v.Z * d;

-                                return tmp;

-                        }

-                        else

-                        {

-                                tmp.Y = radius;

-                                tmp.X = v.X < 0 ? -halfHeight : halfHeight;

-                                tmp.Z = 0;

-                                return tmp;

-                        }

-                }

-        }

-}

+/*
+  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 CylinderShapeX : CylinderShape
+        {
+                public CylinderShapeX(Vector3 halfExtents)
+                        : base(halfExtents) { }
+
+                public override int UpAxis
+                {
+                        get
+                        {
+                                return 0;
+                        }
+                }
+
+                public override float Radius
+                {
+                        get
+                        {
+                                return HalfExtents.Y;
+                        }
+                }
+
+                //debugging
+                public override string Name
+                {
+                        get
+                        {
+                                return "CylinderX";
+                        }
+                }
+
+                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
+                {
+                        return CylinderLocalSupportX(HalfExtents, vec);
+                }
+
+                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
+                {
+                        for (int i = 0; i < vectors.Length; i++)
+                        {
+                                supportVerticesOut[i] = CylinderLocalSupportX(HalfExtents, vectors[i]);
+                        }
+                }
+
+                private Vector3 CylinderLocalSupportX(Vector3 halfExtents, Vector3 v)
+                {
+                        //mapping depends on how cylinder local orientation is
+                        // extents of the cylinder is: X,Y is for radius, and Z for height
+                        float radius = halfExtents.Y;
+                        float halfHeight = halfExtents.X;
+
+                        Vector3 tmp = new Vector3();
+                        float d;
+
+                        float s = (float)Math.Sqrt(v.Y * v.Y + v.Z * v.Z);
+                        if (s != 0)
+                        {
+                                d = radius / s;
+                                tmp.Y = v.Y * d;
+                                tmp.X = v.X < 0 ? -halfHeight : halfHeight;
+                                tmp.Z = v.Z * d;
+                                return tmp;
+                        }
+                        else
+                        {
+                                tmp.Y = radius;
+                                tmp.X = v.X < 0 ? -halfHeight : halfHeight;
+                                tmp.Z = 0;
+                                return tmp;
+                        }
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs
index 1913e9c..10a1995 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs
@@ -1,100 +1,100 @@
-/*

-  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 CylinderShapeZ : CylinderShape

-        {

-                public CylinderShapeZ(Vector3 halfExtents)

-                        : base(halfExtents) { }

-

-                public override int UpAxis

-                {

-                        get

-                        {

-                                return 2;

-                        }

-                }

-

-                public override float Radius

-                {

-                        get

-                        {

-                                return HalfExtents.X;

-                        }

-                }

-

-                //debugging

-                public override string Name

-                {

-                        get

-                        {

-                                return "CylinderZ";

-                        }

-                }

-

-                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)

-                {

-                        return CylinderLocalSupportZ(HalfExtents, vec);

-                }

-

-                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)

-                {

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

-                        {

-                                supportVerticesOut[i] = CylinderLocalSupportZ(HalfExtents, vectors[i]);

-                        }

-                }

-

-                Vector3 CylinderLocalSupportZ(Vector3 halfExtents, Vector3 v)

-                {

-                        //mapping depends on how cylinder local orientation is

-                        // extents of the cylinder is: X,Y is for radius, and Z for height

-                        float radius = halfExtents.X;

-                        float halfHeight = halfExtents.Z;

-

-                        Vector3 tmp = new Vector3();

-                        float d;

-

-                        float s = (float)Math.Sqrt(v.X * v.X + v.Y * v.Y);

-                        if (s != 0)

-                        {

-                                d = radius / s;

-                                tmp.X = v.X * d;

-                                tmp.Z = v.Z < 0 ? -halfHeight : halfHeight;

-                                tmp.Y = v.Y * d;

-                                return tmp;

-                        }

-                        else

-                        {

-                                tmp.X = radius;

-                                tmp.Z = v.Z < 0 ? -halfHeight : halfHeight;

-                                tmp.Y = 0;

-                                return tmp;

-                        }

-                }

-        }

-}

+/*
+  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 CylinderShapeZ : CylinderShape
+        {
+                public CylinderShapeZ(Vector3 halfExtents)
+                        : base(halfExtents) { }
+
+                public override int UpAxis
+                {
+                        get
+                        {
+                                return 2;
+                        }
+                }
+
+                public override float Radius
+                {
+                        get
+                        {
+                                return HalfExtents.X;
+                        }
+                }
+
+                //debugging
+                public override string Name
+                {
+                        get
+                        {
+                                return "CylinderZ";
+                        }
+                }
+
+                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
+                {
+                        return CylinderLocalSupportZ(HalfExtents, vec);
+                }
+
+                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
+                {
+                        for (int i = 0; i < vectors.Length; i++)
+                        {
+                                supportVerticesOut[i] = CylinderLocalSupportZ(HalfExtents, vectors[i]);
+                        }
+                }
+
+                Vector3 CylinderLocalSupportZ(Vector3 halfExtents, Vector3 v)
+                {
+                        //mapping depends on how cylinder local orientation is
+                        // extents of the cylinder is: X,Y is for radius, and Z for height
+                        float radius = halfExtents.X;
+                        float halfHeight = halfExtents.Z;
+
+                        Vector3 tmp = new Vector3();
+                        float d;
+
+                        float s = (float)Math.Sqrt(v.X * v.X + v.Y * v.Y);
+                        if (s != 0)
+                        {
+                                d = radius / s;
+                                tmp.X = v.X * d;
+                                tmp.Z = v.Z < 0 ? -halfHeight : halfHeight;
+                                tmp.Y = v.Y * d;
+                                return tmp;
+                        }
+                        else
+                        {
+                                tmp.X = radius;
+                                tmp.Z = v.Z < 0 ? -halfHeight : halfHeight;
+                                tmp.Y = 0;
+                                return tmp;
+                        }
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs
index b094c39..ef6f134 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs
@@ -1,80 +1,80 @@
-/*

-  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;

-using System.Diagnostics;

-

-namespace XnaDevRu.BulletX

-{

-    public class EmptyShape : ConcaveShape

-    {

-        private Vector3 _localScaling;

-

-                public override string Name

-                {

-                        get

-                        {

-                                return "Empty";

-                        }

-                }

-

-                public override BroadphaseNativeTypes ShapeType

-                {

-                        get

-                        {

-                                return BroadphaseNativeTypes.Empty;

-                        }

-                }

-

-                public override Vector3 LocalScaling

-                {

-                        get

-                        {

-                                return _localScaling;

-                        }

-                        set

-                        {

-                                _localScaling = value;

-                        }

-                }

-

-        public override void ProcessAllTriangles(ITriangleCallback callback, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax)

-        {

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

-        }

-

-        public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)

-        {

-                        Vector3 margin = new Vector3(Margin, Margin, Margin);

-            aabbMin = t.Translation - margin;

-            aabbMax = t.Translation + margin;

-        }

-

-        public override void CalculateLocalInertia(float mass, out Vector3 inertia)

-        {

-            inertia = new Vector3();

-            BulletDebug.Assert(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;
+using System.Diagnostics;
+
+namespace XnaDevRu.BulletX
+{
+    public class EmptyShape : ConcaveShape
+    {
+        private Vector3 _localScaling;
+
+                public override string Name
+                {
+                        get
+                        {
+                                return "Empty";
+                        }
+                }
+
+                public override BroadphaseNativeTypes ShapeType
+                {
+                        get
+                        {
+                                return BroadphaseNativeTypes.Empty;
+                        }
+                }
+
+                public override Vector3 LocalScaling
+                {
+                        get
+                        {
+                                return _localScaling;
+                        }
+                        set
+                        {
+                                _localScaling = value;
+                        }
+                }
+
+        public override void ProcessAllTriangles(ITriangleCallback callback, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax)
+        {
+            throw new Exception("The method or operation is not implemented.");
+        }
+
+        public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
+        {
+                        Vector3 margin = new Vector3(Margin, Margin, Margin);
+            aabbMin = t.Translation - margin;
+            aabbMax = t.Translation + margin;
+        }
+
+        public override void CalculateLocalInertia(float mass, out Vector3 inertia)
+        {
+            inertia = new Vector3();
+            BulletDebug.Assert(false);
+        }
+    }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs
index 5073096..dffd865 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs
@@ -1,55 +1,55 @@
-/*

-  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 FilteredCallback : ITriangleIndexCallback

-        {

-                private ITriangleCallback _callback;

-                private Vector3 _aabbMin;

-                private Vector3 _aabbMax;

-

-                public FilteredCallback(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)

-                {

-                        _callback = callback;

-                        _aabbMin = aabbMin;

-                        _aabbMax = aabbMax;

-                }

-

-                public ITriangleCallback TriangleCallback { get { return _callback; } set { _callback = value; } }

-                public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } }

-                public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } }

-

-                public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex)

-                {

-                        if (MathHelper.TestTriangleAgainstAabb2(triangle, _aabbMin, _aabbMax))

-                        {

-                                //check aabb in triangle-space, before doing this

-                                _callback.ProcessTriangle(triangle, partId, triangleIndex);

-                        }

-                }

-        }

-}

+/*
+  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 FilteredCallback : ITriangleIndexCallback
+        {
+                private ITriangleCallback _callback;
+                private Vector3 _aabbMin;
+                private Vector3 _aabbMax;
+
+                public FilteredCallback(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
+                {
+                        _callback = callback;
+                        _aabbMin = aabbMin;
+                        _aabbMax = aabbMax;
+                }
+
+                public ITriangleCallback TriangleCallback { get { return _callback; } set { _callback = value; } }
+                public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } }
+                public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } }
+
+                public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex)
+                {
+                        if (MathHelper.TestTriangleAgainstAabb2(triangle, _aabbMin, _aabbMax))
+                        {
+                                //check aabb in triangle-space, before doing this
+                                _callback.ProcessTriangle(triangle, partId, triangleIndex);
+                        }
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs
index b88d804..51c65b7 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs
@@ -1,33 +1,33 @@
-/*

-  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 interface ITriangleIndexCallback

-    {

-        void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex);

-    }

-}

+/*
+  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 interface ITriangleIndexCallback
+    {
+        void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex);
+    }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs
index 951707d..1e5e141 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs
@@ -1,58 +1,58 @@
-/*

-  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 LocalSupportVertexCallback : ITriangleIndexCallback

-        {

-                private Vector3 _supportVertexLocal;

-                private float _maxDot;

-                private Vector3 _supportVecLocal;

-

-                public LocalSupportVertexCallback(Vector3 supportVecLocal)

-                {

-                        _supportVertexLocal = new Vector3();

-                        _maxDot = -1e30f;

-                        _supportVecLocal = supportVecLocal;

-                }

-

-                public float MaxDot { get { return _maxDot; } set { _maxDot = value; } }

-                public Vector3 SupportVertexLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } }

-

-                public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex)

-                {

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

-                        {

-                                float dot = Vector3.Dot(_supportVecLocal, triangle[i]);

-                                if (dot > _maxDot)

-                                {

-                                        _maxDot = dot;

-                                        _supportVertexLocal = triangle[i];

-                                }

-                        }

-                }

-        }

-}

+/*
+  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 LocalSupportVertexCallback : ITriangleIndexCallback
+        {
+                private Vector3 _supportVertexLocal;
+                private float _maxDot;
+                private Vector3 _supportVecLocal;
+
+                public LocalSupportVertexCallback(Vector3 supportVecLocal)
+                {
+                        _supportVertexLocal = new Vector3();
+                        _maxDot = -1e30f;
+                        _supportVecLocal = supportVecLocal;
+                }
+
+                public float MaxDot { get { return _maxDot; } set { _maxDot = value; } }
+                public Vector3 SupportVertexLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } }
+
+                public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex)
+                {
+                        for (int i = 0; i < 3; i++)
+                        {
+                                float dot = Vector3.Dot(_supportVecLocal, triangle[i]);
+                                if (dot > _maxDot)
+                                {
+                                        _maxDot = dot;
+                                        _supportVertexLocal = triangle[i];
+                                }
+                        }
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs
index 6f70cae..8cdcfbe 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs
@@ -1,99 +1,99 @@
-/*

-  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>

-        /// MinkowskiSumShape represents implicit (getSupportingVertex) based minkowski sum of two convex implicit shapes.

-        /// </summary>

-        public class MinkowskiSumShape : ConvexShape

-        {

-                private Matrix _transformA;

-                private Matrix _transformB;

-                private ConvexShape _shapeA;

-                private ConvexShape _shapeB;

-

-                public MinkowskiSumShape(ConvexShape shapeA, ConvexShape shapeB)

-                {

-                        _shapeA = shapeA;

-                        _shapeB = shapeB;

-                        _transformA = Matrix.Identity;

-                        _transformB = Matrix.Identity;

-                }

-

-                public Matrix TransformA { get { return _transformA; } set { _transformA = value; } }

-                public Matrix TransformB { get { return _transformB; } set { _transformB = value; } }

-                public ConvexShape ShapeA { get { return _shapeA; } }

-                public ConvexShape ShapeB { get { return _shapeB; } }

-

-                public override float Margin

-                {

-                        get

-                        {

-                                return _shapeA.Margin + _shapeB.Margin;

-                        }

-                        set

-                        {

-                                base.Margin = value;

-                        }

-                }

-

-                public override BroadphaseNativeTypes ShapeType

-                {

-                        get

-                        {

-                                return BroadphaseNativeTypes.MinkowskiDifference;

-                        }

-                }

-

-                public override string Name

-                {

-                        get

-                        {

-                                return "MinkowskiSum";

-                        }

-                }

-

-                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)

-                {

-                        Vector3 supVertexA = MathHelper.MatrixToVector(_transformA, _shapeA.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformA)));

-                        Vector3 supVertexB = MathHelper.MatrixToVector(_transformB, _shapeB.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformB)));

-                        return supVertexA + supVertexB;

-                }

-

-                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)

-                {

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

-                                supportVerticesOut[i] = LocalGetSupportingVertexWithoutMargin(vectors[i]);

-                }

-

-                public override void CalculateLocalInertia(float mass, out Vector3 inertia)

-                {

-                        inertia = new Vector3();

-                        BulletDebug.Assert(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>
+        /// MinkowskiSumShape represents implicit (getSupportingVertex) based minkowski sum of two convex implicit shapes.
+        /// </summary>
+        public class MinkowskiSumShape : ConvexShape
+        {
+                private Matrix _transformA;
+                private Matrix _transformB;
+                private ConvexShape _shapeA;
+                private ConvexShape _shapeB;
+
+                public MinkowskiSumShape(ConvexShape shapeA, ConvexShape shapeB)
+                {
+                        _shapeA = shapeA;
+                        _shapeB = shapeB;
+                        _transformA = Matrix.Identity;
+                        _transformB = Matrix.Identity;
+                }
+
+                public Matrix TransformA { get { return _transformA; } set { _transformA = value; } }
+                public Matrix TransformB { get { return _transformB; } set { _transformB = value; } }
+                public ConvexShape ShapeA { get { return _shapeA; } }
+                public ConvexShape ShapeB { get { return _shapeB; } }
+
+                public override float Margin
+                {
+                        get
+                        {
+                                return _shapeA.Margin + _shapeB.Margin;
+                        }
+                        set
+                        {
+                                base.Margin = value;
+                        }
+                }
+
+                public override BroadphaseNativeTypes ShapeType
+                {
+                        get
+                        {
+                                return BroadphaseNativeTypes.MinkowskiDifference;
+                        }
+                }
+
+                public override string Name
+                {
+                        get
+                        {
+                                return "MinkowskiSum";
+                        }
+                }
+
+                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
+                {
+                        Vector3 supVertexA = MathHelper.MatrixToVector(_transformA, _shapeA.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformA)));
+                        Vector3 supVertexB = MathHelper.MatrixToVector(_transformB, _shapeB.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformB)));
+                        return supVertexA + supVertexB;
+                }
+
+                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
+                {
+                        for (int i = 0; i < vectors.Length; i++)
+                                supportVerticesOut[i] = LocalGetSupportingVertexWithoutMargin(vectors[i]);
+                }
+
+                public override void CalculateLocalInertia(float mass, out Vector3 inertia)
+                {
+                        inertia = new Vector3();
+                        BulletDebug.Assert(false);
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs
index 9959c01..73fd094 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs
@@ -1,154 +1,154 @@
-/*

-  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>

-        /// MultiSphereShape represents implicit convex hull of a collection of spheres (using getSupportingVertex)

-        /// </summary>

-        public class MultiSphereShape : ConvexShape

-        {

-                private const int _maxNumSpheres = 5;

-                private Vector3[] _localPositions = new Vector3[MaxNumSpheres];

-                private float[] _radi = new float[MaxNumSpheres];

-                private Vector3 _inertiaHalfExtents;

-

-                private int m_numSpheres;

-

-                public MultiSphereShape(Vector3 inertiaHalfExtents, Vector3[] positions, float[] radi, int numSpheres)

-                {

-                        _inertiaHalfExtents = inertiaHalfExtents;

-                        float startMargin = 1e30f;

-

-                        m_numSpheres = numSpheres;

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

-                        {

-                                _localPositions[i] = positions[i];

-                                _radi[i] = radi[i];

-                                if (radi[i] < startMargin)

-                                        startMargin = radi[i];

-                        }

-                        Margin = startMargin;

-                }

-

-                public static int MaxNumSpheres { get { return _maxNumSpheres; } }

-

-                public override BroadphaseNativeTypes ShapeType

-                {

-                        get

-                        {

-                                return BroadphaseNativeTypes.MultiSphere;

-                        }

-                }

-

-                public override string Name

-                {

-                        get

-                        {

-                                return "MultiSphere";

-                        }

-                }

-

-                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vecA)

-                {

-                        Vector3 supVec = new Vector3();

-

-                        float maxDot = -1e30f;

-

-

-                        Vector3 vec = vecA;

-                        float lenSqr = vec.LengthSquared();

-                        if (lenSqr < 0.0001f)

-                        {

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

-                        }

-                        else

-                        {

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

-                                vec *= rlen;

-                        }

-

-                        Vector3 vtx;

-                        float newDot;

-

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

-                        {

-                                vtx = _localPositions[i] + vec * LocalScaling * _radi[i] - vec * Margin;

-                                newDot = Vector3.Dot(vec, vtx);

-                                if (newDot > maxDot)

-                                {

-                                        maxDot = newDot;

-                                        supVec = vtx;

-                                }

-                        }

-

-                        return supVec;

-                }

-

-                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)

-                {

-                        for (int j = 0; j < vectors.Length; j++)

-                        {

-                                float maxDot = -1e30f;

-                                Vector3 vtx;

-                                float newDot;

-

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

-                                {

-                                        vtx = _localPositions[i] + vectors[j] * LocalScaling * _radi[i] - vectors[j] * Margin;

-                                        newDot = Vector3.Dot(vectors[j], vtx);

-                                        if (newDot > maxDot)

-                                        {

-                                                maxDot = newDot;

-                                                supportVerticesOut[j] = vtx;

-                                        }

-                                }

-                        }

-                }

-

-                public override void CalculateLocalInertia(float mass, out Vector3 inertia)

-                {

-                        //as an approximation, take the inertia of the box that bounds the spheres

-                        Matrix ident = Matrix.Identity;

-                        Vector3 halfExtents = _inertiaHalfExtents;

-

-                        float margin = ConvexDistanceMargin;

-

-                        float lx = 2f * (halfExtents.X + margin);

-                        float ly = 2f * (halfExtents.Y + margin);

-                        float lz = 2f * (halfExtents.Z + margin);

-                        float x2 = lx * lx;

-                        float y2 = ly * ly;

-                        float z2 = lz * lz;

-                        float scaledmass = mass * 0.08333333f;

-

-                        inertia = new Vector3();

-                        inertia.X = scaledmass * (y2 + z2);

-                        inertia.Y = scaledmass * (x2 + z2);

-                        inertia.Z = scaledmass * (x2 + y2);

-                }

-        }

-}

+/*
+  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>
+        /// MultiSphereShape represents implicit convex hull of a collection of spheres (using getSupportingVertex)
+        /// </summary>
+        public class MultiSphereShape : ConvexShape
+        {
+                private const int _maxNumSpheres = 5;
+                private Vector3[] _localPositions = new Vector3[MaxNumSpheres];
+                private float[] _radi = new float[MaxNumSpheres];
+                private Vector3 _inertiaHalfExtents;
+
+                private int m_numSpheres;
+
+                public MultiSphereShape(Vector3 inertiaHalfExtents, Vector3[] positions, float[] radi, int numSpheres)
+                {
+                        _inertiaHalfExtents = inertiaHalfExtents;
+                        float startMargin = 1e30f;
+
+                        m_numSpheres = numSpheres;
+                        for (int i = 0; i < m_numSpheres; i++)
+                        {
+                                _localPositions[i] = positions[i];
+                                _radi[i] = radi[i];
+                                if (radi[i] < startMargin)
+                                        startMargin = radi[i];
+                        }
+                        Margin = startMargin;
+                }
+
+                public static int MaxNumSpheres { get { return _maxNumSpheres; } }
+
+                public override BroadphaseNativeTypes ShapeType
+                {
+                        get
+                        {
+                                return BroadphaseNativeTypes.MultiSphere;
+                        }
+                }
+
+                public override string Name
+                {
+                        get
+                        {
+                                return "MultiSphere";
+                        }
+                }
+
+                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vecA)
+                {
+                        Vector3 supVec = new Vector3();
+
+                        float maxDot = -1e30f;
+
+
+                        Vector3 vec = vecA;
+                        float lenSqr = vec.LengthSquared();
+                        if (lenSqr < 0.0001f)
+                        {
+                                vec = new Vector3(1, 0, 0);
+                        }
+                        else
+                        {
+                                float rlen = 1f / (float)Math.Sqrt(lenSqr);
+                                vec *= rlen;
+                        }
+
+                        Vector3 vtx;
+                        float newDot;
+
+                        for (int i = 0; i < m_numSpheres; i++)
+                        {
+                                vtx = _localPositions[i] + vec * LocalScaling * _radi[i] - vec * Margin;
+                                newDot = Vector3.Dot(vec, vtx);
+                                if (newDot > maxDot)
+                                {
+                                        maxDot = newDot;
+                                        supVec = vtx;
+                                }
+                        }
+
+                        return supVec;
+                }
+
+                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
+                {
+                        for (int j = 0; j < vectors.Length; j++)
+                        {
+                                float maxDot = -1e30f;
+                                Vector3 vtx;
+                                float newDot;
+
+                                for (int i = 0; i < m_numSpheres; i++)
+                                {
+                                        vtx = _localPositions[i] + vectors[j] * LocalScaling * _radi[i] - vectors[j] * Margin;
+                                        newDot = Vector3.Dot(vectors[j], vtx);
+                                        if (newDot > maxDot)
+                                        {
+                                                maxDot = newDot;
+                                                supportVerticesOut[j] = vtx;
+                                        }
+                                }
+                        }
+                }
+
+                public override void CalculateLocalInertia(float mass, out Vector3 inertia)
+                {
+                        //as an approximation, take the inertia of the box that bounds the spheres
+                        Matrix ident = Matrix.Identity;
+                        Vector3 halfExtents = _inertiaHalfExtents;
+
+                        float margin = ConvexDistanceMargin;
+
+                        float lx = 2f * (halfExtents.X + margin);
+                        float ly = 2f * (halfExtents.Y + margin);
+                        float lz = 2f * (halfExtents.Z + margin);
+                        float x2 = lx * lx;
+                        float y2 = ly * ly;
+                        float z2 = lz * lz;
+                        float scaledmass = mass * 0.08333333f;
+
+                        inertia = new Vector3();
+                        inertia.X = scaledmass * (y2 + z2);
+                        inertia.Y = scaledmass * (x2 + z2);
+                        inertia.Z = scaledmass * (x2 + y2);
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs
index fac344e..324cdbc 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs
@@ -1,32 +1,32 @@
-/*

-  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;

-

-namespace XnaDevRu.BulletX

-{

-        public interface INodeOverlapCallback

-        {

-                void ProcessNode(OptimizedBvhNode node);

-        }

-}

+/*
+  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;
+
+namespace XnaDevRu.BulletX
+{
+        public interface INodeOverlapCallback
+        {
+                void ProcessNode(OptimizedBvhNode node);
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs
index c22c990..5b7a5e4 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs
@@ -1,293 +1,293 @@
-/*

-  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>

-        /// OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future)

-        /// </summary>

-        public class OptimizedBvh

-        {

-                private static int _maxIterations = 0;

-                private OptimizedBvhNode _rootNode;

-

-                private OptimizedBvhNode[] _contiguousNodes;

-                private int _curNodeIndex;

-

-                private List<OptimizedBvhNode> _leafNodes = new List<OptimizedBvhNode>();

-

-        public OptimizedBvh() { }

-

-                public void Build(StridingMeshInterface triangles)

-                {

-                        NodeTriangleCallback callback = new NodeTriangleCallback(_leafNodes);

-

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

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

-

-                        triangles.InternalProcessAllTriangles(callback, aabbMin, aabbMax);

-

-                        //now we have an array of leafnodes in m_leafNodes

-                        _contiguousNodes = new OptimizedBvhNode[2 * _leafNodes.Count];

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

-                _contiguousNodes[i] = new OptimizedBvhNode();

-                        _curNodeIndex = 0;

-

-                        _rootNode = BuildTree(_leafNodes, 0, _leafNodes.Count);

-                }

-

-                public OptimizedBvhNode BuildTree(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex)

-                {

-                        OptimizedBvhNode internalNode;

-

-                        int splitAxis, splitIndex, i;

-                        int numIndices = endIndex - startIndex;

-                        int curIndex = _curNodeIndex;

-

-                        if (numIndices <= 0)

-                                throw new BulletException();

-

-                        if (numIndices == 1)

-                        {

-                                _contiguousNodes[_curNodeIndex++] = leafNodes[startIndex];

-                                //return new (&m_contiguousNodes[m_curNodeIndex++]) btOptimizedBvhNode(leafNodes[startIndex]);

-                                return leafNodes[startIndex];

-                        }

-

-                        //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.

-                        splitAxis = CalculateSplittingAxis(leafNodes, startIndex, endIndex);

-

-                        splitIndex = SortAndCalculateSplittingIndex(leafNodes, startIndex, endIndex, splitAxis);

-

-                        internalNode = _contiguousNodes[_curNodeIndex++];

-

-                        internalNode.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f);

-                        internalNode.AabbMin = new Vector3(1e30f, 1e30f, 1e30f);

-

-                        for (i = startIndex; i < endIndex; i++)

-                        {

-                                internalNode.AabbMax = MathHelper.SetMax(internalNode.AabbMax, leafNodes[i].AabbMax);

-                                internalNode.AabbMin = MathHelper.SetMin(internalNode.AabbMin, leafNodes[i].AabbMin);

-                        }

-

-                        //internalNode->m_escapeIndex;

-                        internalNode.LeftChild = BuildTree(leafNodes, startIndex, splitIndex);

-                        internalNode.RightChild = BuildTree(leafNodes, splitIndex, endIndex);

-

-                        internalNode.EscapeIndex = _curNodeIndex - curIndex;

-                        return internalNode;

-                }

-

-                public int CalculateSplittingAxis(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex)

-                {

-                        Vector3 means = new Vector3();

-                        Vector3 variance = new Vector3();

-                        int numIndices = endIndex - startIndex;

-

-                        for (int i = startIndex; i < endIndex; i++)

-                        {

-                                Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin);

-                                means += center;

-                        }

-                        means *= (1f / (float)numIndices);

-

-                        for (int i = startIndex; i < endIndex; i++)

-                        {

-                                Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin);

-                                Vector3 diff2 = center - means;

-                                diff2 = diff2 * diff2;

-                                variance += diff2;

-                        }

-                        variance *= (1f / ((float)numIndices - 1));

-

-                        return MathHelper.MaxAxis(variance);

-                }

-

-                public int SortAndCalculateSplittingIndex(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex, int splitAxis)

-                {

-                        int splitIndex = startIndex;

-                        int numIndices = endIndex - startIndex;

-                        float splitValue;

-

-                        Vector3 means = new Vector3();

-                        for (int i = startIndex; i < endIndex; i++)

-                        {

-                                Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin);

-                                means += center;

-                        }

-                        means *= (1f / (float)numIndices);

-

-                        if (splitAxis == 0)

-                                splitValue = means.X;

-                        else if (splitAxis == 1)

-                                splitValue = means.Y;

-                        else if (splitAxis == 2)

-                                splitValue = means.Z;

-                        else

-                                throw new ArgumentException();

-

-                        //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.

-                        for (int i = startIndex; i < endIndex; i++)

-                        {

-                                Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin);

-                                float centerSplit;

-

-                                if (splitAxis == 0)

-                                        centerSplit = means.X;

-                                else if (splitAxis == 1)

-                                        centerSplit = means.Y;

-                                else if (splitAxis == 2)

-                                        centerSplit = means.Z;

-                                else

-                                        throw new ArgumentException();

-

-                                if (centerSplit > splitValue)

-                                {

-                                        //swap

-                                        OptimizedBvhNode tmp = leafNodes[i];

-                                        leafNodes[i] = leafNodes[splitIndex];

-                                        leafNodes[splitIndex] = tmp;

-                                        splitIndex++;

-                                }

-                        }

-                        if ((splitIndex == startIndex) || (splitIndex == (endIndex - 1)))

-                        {

-                                splitIndex = startIndex + (numIndices >> 1);

-                        }

-                        return splitIndex;

-                }

-

-                public void WalkTree(OptimizedBvhNode rootNode, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax)

-                {

-                        bool isLeafNode, aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax);

-                        if (aabbOverlap)

-                        {

-                                isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null);

-                                if (isLeafNode)

-                                {

-                                        nodeCallback.ProcessNode(rootNode);

-                                }

-                                else

-                                {

-                                        WalkTree(rootNode.LeftChild, nodeCallback, aabbMin, aabbMax);

-                                        WalkTree(rootNode.RightChild, nodeCallback, aabbMin, aabbMax);

-                                }

-                        }

-                }

-

-                public void WalkStacklessTree(OptimizedBvhNode[] rootNodeArray, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax)

-                {

-                        int escapeIndex, curIndex = 0;

-                        int walkIterations = 0;

-                        bool aabbOverlap, isLeafNode;

-            int rootNodeIndex = 0;

-            OptimizedBvhNode rootNode = rootNodeArray[rootNodeIndex];

-

-                        while (curIndex < _curNodeIndex)

-                        {

-                                //catch bugs in tree data

-                                if (walkIterations >= _curNodeIndex)

-                                        throw new BulletException();

-

-                                walkIterations++;

-                                aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax);

-                                isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null);

-

-                                if (isLeafNode && aabbOverlap)

-                                {

-                                        nodeCallback.ProcessNode(rootNode);

-                                }

-

-                                if (aabbOverlap || isLeafNode)

-                                {

-                                        rootNodeIndex++; // this

-                                        curIndex++;

-                    if (rootNodeIndex < rootNodeArray.Length)

-                        rootNode = rootNodeArray[rootNodeIndex];

-                }

-                                else

-                                {

-                                        escapeIndex = rootNode.EscapeIndex;

-                                        rootNodeIndex += escapeIndex; // and this

-                                        curIndex += escapeIndex;

-                    if (rootNodeIndex < rootNodeArray.Length)

-                        rootNode = rootNodeArray[rootNodeIndex];

-                                }

-

-                        }

-

-                        if (_maxIterations < walkIterations)

-                                _maxIterations = walkIterations;

-                }

-

-                public void ReportAabbOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax)

-                {

-                        //either choose recursive traversal (walkTree) or stackless (walkStacklessTree)

-                        //walkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax);

-                        //WalkStacklessTree(_rootNode, nodeCallback, aabbMin, aabbMax);

-            WalkStacklessTree(_contiguousNodes, nodeCallback, aabbMin, aabbMax);

-                }

-

-                public void ReportSphereOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) { }

-        }

-

-        public class NodeTriangleCallback : ITriangleIndexCallback

-        {

-                private List<OptimizedBvhNode> _triangleNodes;

-

-                public NodeTriangleCallback(List<OptimizedBvhNode> triangleNodes)

-                {

-                        _triangleNodes = triangleNodes;

-                }

-

-                public List<OptimizedBvhNode> TriangleNodes { get { return _triangleNodes; } set { _triangleNodes = value; } }

-

-                public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex)

-                {

-

-                        OptimizedBvhNode node = new OptimizedBvhNode();

-                        node.AabbMin = new Vector3(1e30f, 1e30f, 1e30f);

-                        node.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f);

-

-                        node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[0]);

-                        node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[0]);

-                        node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[1]);

-                        node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[1]);

-                        node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[2]);

-                        node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[2]);

-

-                        node.EscapeIndex = -1;

-                        node.LeftChild = null;

-                        node.RightChild = null;

-

-                        //for child nodes

-                        node.SubPart = partId;

-                        node.TriangleIndex = triangleIndex;

-

-                        _triangleNodes.Add(node);

-                }

-        }

-}

+/*
+  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>
+        /// OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future)
+        /// </summary>
+        public class OptimizedBvh
+        {
+                private static int _maxIterations = 0;
+                private OptimizedBvhNode _rootNode;
+
+                private OptimizedBvhNode[] _contiguousNodes;
+                private int _curNodeIndex;
+
+                private List<OptimizedBvhNode> _leafNodes = new List<OptimizedBvhNode>();
+
+        public OptimizedBvh() { }
+
+                public void Build(StridingMeshInterface triangles)
+                {
+                        NodeTriangleCallback callback = new NodeTriangleCallback(_leafNodes);
+
+                        Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
+                        Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
+
+                        triangles.InternalProcessAllTriangles(callback, aabbMin, aabbMax);
+
+                        //now we have an array of leafnodes in m_leafNodes
+                        _contiguousNodes = new OptimizedBvhNode[2 * _leafNodes.Count];
+            for (int i = 0; i < _contiguousNodes.Length; i++)
+                _contiguousNodes[i] = new OptimizedBvhNode();
+                        _curNodeIndex = 0;
+
+                        _rootNode = BuildTree(_leafNodes, 0, _leafNodes.Count);
+                }
+
+                public OptimizedBvhNode BuildTree(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex)
+                {
+                        OptimizedBvhNode internalNode;
+
+                        int splitAxis, splitIndex, i;
+                        int numIndices = endIndex - startIndex;
+                        int curIndex = _curNodeIndex;
+
+                        if (numIndices <= 0)
+                                throw new BulletException();
+
+                        if (numIndices == 1)
+                        {
+                                _contiguousNodes[_curNodeIndex++] = leafNodes[startIndex];
+                                //return new (&m_contiguousNodes[m_curNodeIndex++]) btOptimizedBvhNode(leafNodes[startIndex]);
+                                return leafNodes[startIndex];
+                        }
+
+                        //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
+                        splitAxis = CalculateSplittingAxis(leafNodes, startIndex, endIndex);
+
+                        splitIndex = SortAndCalculateSplittingIndex(leafNodes, startIndex, endIndex, splitAxis);
+
+                        internalNode = _contiguousNodes[_curNodeIndex++];
+
+                        internalNode.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f);
+                        internalNode.AabbMin = new Vector3(1e30f, 1e30f, 1e30f);
+
+                        for (i = startIndex; i < endIndex; i++)
+                        {
+                                internalNode.AabbMax = MathHelper.SetMax(internalNode.AabbMax, leafNodes[i].AabbMax);
+                                internalNode.AabbMin = MathHelper.SetMin(internalNode.AabbMin, leafNodes[i].AabbMin);
+                        }
+
+                        //internalNode->m_escapeIndex;
+                        internalNode.LeftChild = BuildTree(leafNodes, startIndex, splitIndex);
+                        internalNode.RightChild = BuildTree(leafNodes, splitIndex, endIndex);
+
+                        internalNode.EscapeIndex = _curNodeIndex - curIndex;
+                        return internalNode;
+                }
+
+                public int CalculateSplittingAxis(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex)
+                {
+                        Vector3 means = new Vector3();
+                        Vector3 variance = new Vector3();
+                        int numIndices = endIndex - startIndex;
+
+                        for (int i = startIndex; i < endIndex; i++)
+                        {
+                                Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin);
+                                means += center;
+                        }
+                        means *= (1f / (float)numIndices);
+
+                        for (int i = startIndex; i < endIndex; i++)
+                        {
+                                Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin);
+                                Vector3 diff2 = center - means;
+                                diff2 = diff2 * diff2;
+                                variance += diff2;
+                        }
+                        variance *= (1f / ((float)numIndices - 1));
+
+                        return MathHelper.MaxAxis(variance);
+                }
+
+                public int SortAndCalculateSplittingIndex(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex, int splitAxis)
+                {
+                        int splitIndex = startIndex;
+                        int numIndices = endIndex - startIndex;
+                        float splitValue;
+
+                        Vector3 means = new Vector3();
+                        for (int i = startIndex; i < endIndex; i++)
+                        {
+                                Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin);
+                                means += center;
+                        }
+                        means *= (1f / (float)numIndices);
+
+                        if (splitAxis == 0)
+                                splitValue = means.X;
+                        else if (splitAxis == 1)
+                                splitValue = means.Y;
+                        else if (splitAxis == 2)
+                                splitValue = means.Z;
+                        else
+                                throw new ArgumentException();
+
+                        //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
+                        for (int i = startIndex; i < endIndex; i++)
+                        {
+                                Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin);
+                                float centerSplit;
+
+                                if (splitAxis == 0)
+                                        centerSplit = means.X;
+                                else if (splitAxis == 1)
+                                        centerSplit = means.Y;
+                                else if (splitAxis == 2)
+                                        centerSplit = means.Z;
+                                else
+                                        throw new ArgumentException();
+
+                                if (centerSplit > splitValue)
+                                {
+                                        //swap
+                                        OptimizedBvhNode tmp = leafNodes[i];
+                                        leafNodes[i] = leafNodes[splitIndex];
+                                        leafNodes[splitIndex] = tmp;
+                                        splitIndex++;
+                                }
+                        }
+                        if ((splitIndex == startIndex) || (splitIndex == (endIndex - 1)))
+                        {
+                                splitIndex = startIndex + (numIndices >> 1);
+                        }
+                        return splitIndex;
+                }
+
+                public void WalkTree(OptimizedBvhNode rootNode, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax)
+                {
+                        bool isLeafNode, aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax);
+                        if (aabbOverlap)
+                        {
+                                isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null);
+                                if (isLeafNode)
+                                {
+                                        nodeCallback.ProcessNode(rootNode);
+                                }
+                                else
+                                {
+                                        WalkTree(rootNode.LeftChild, nodeCallback, aabbMin, aabbMax);
+                                        WalkTree(rootNode.RightChild, nodeCallback, aabbMin, aabbMax);
+                                }
+                        }
+                }
+
+                public void WalkStacklessTree(OptimizedBvhNode[] rootNodeArray, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax)
+                {
+                        int escapeIndex, curIndex = 0;
+                        int walkIterations = 0;
+                        bool aabbOverlap, isLeafNode;
+            int rootNodeIndex = 0;
+            OptimizedBvhNode rootNode = rootNodeArray[rootNodeIndex];
+
+                        while (curIndex < _curNodeIndex)
+                        {
+                                //catch bugs in tree data
+                                if (walkIterations >= _curNodeIndex)
+                                        throw new BulletException();
+
+                                walkIterations++;
+                                aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax);
+                                isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null);
+
+                                if (isLeafNode && aabbOverlap)
+                                {
+                                        nodeCallback.ProcessNode(rootNode);
+                                }
+
+                                if (aabbOverlap || isLeafNode)
+                                {
+                                        rootNodeIndex++; // this
+                                        curIndex++;
+                    if (rootNodeIndex < rootNodeArray.Length)
+                        rootNode = rootNodeArray[rootNodeIndex];
+                }
+                                else
+                                {
+                                        escapeIndex = rootNode.EscapeIndex;
+                                        rootNodeIndex += escapeIndex; // and this
+                                        curIndex += escapeIndex;
+                    if (rootNodeIndex < rootNodeArray.Length)
+                        rootNode = rootNodeArray[rootNodeIndex];
+                                }
+
+                        }
+
+                        if (_maxIterations < walkIterations)
+                                _maxIterations = walkIterations;
+                }
+
+                public void ReportAabbOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax)
+                {
+                        //either choose recursive traversal (walkTree) or stackless (walkStacklessTree)
+                        //walkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax);
+                        //WalkStacklessTree(_rootNode, nodeCallback, aabbMin, aabbMax);
+            WalkStacklessTree(_contiguousNodes, nodeCallback, aabbMin, aabbMax);
+                }
+
+                public void ReportSphereOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) { }
+        }
+
+        public class NodeTriangleCallback : ITriangleIndexCallback
+        {
+                private List<OptimizedBvhNode> _triangleNodes;
+
+                public NodeTriangleCallback(List<OptimizedBvhNode> triangleNodes)
+                {
+                        _triangleNodes = triangleNodes;
+                }
+
+                public List<OptimizedBvhNode> TriangleNodes { get { return _triangleNodes; } set { _triangleNodes = value; } }
+
+                public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex)
+                {
+
+                        OptimizedBvhNode node = new OptimizedBvhNode();
+                        node.AabbMin = new Vector3(1e30f, 1e30f, 1e30f);
+                        node.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f);
+
+                        node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[0]);
+                        node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[0]);
+                        node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[1]);
+                        node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[1]);
+                        node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[2]);
+                        node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[2]);
+
+                        node.EscapeIndex = -1;
+                        node.LeftChild = null;
+                        node.RightChild = null;
+
+                        //for child nodes
+                        node.SubPart = partId;
+                        node.TriangleIndex = triangleIndex;
+
+                        _triangleNodes.Add(node);
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs
index d43f2fb..83cc6e5 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs
@@ -1,63 +1,63 @@
-/*

-  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>

-        /// OptimizedBvhNode contains both internal and leaf node information.

-        /// It hasn't been optimized yet for storage. Some obvious optimizations are:

-        /// Removal of the pointers (can already be done, they are not used for traversal)

-        /// and storing aabbmin/max as quantized integers.

-        /// 'subpart' doesn't need an integer either. It allows to re-use graphics triangle

-        /// meshes stored in a non-uniform way (like batches/subparts of triangle-fans

-        /// </summary>

-        public class OptimizedBvhNode

-        {

-                private Vector3 _aabbMin;

-                private Vector3 _aabbMax;

-

-                //these 2 pointers are obsolete, the stackless traversal just uses the escape index

-                private OptimizedBvhNode _leftChild;

-                private OptimizedBvhNode _rightChild;

-

-                private int _escapeIndex;

-

-                //for child nodes

-                private int _subPart;

-                private int _triangleIndex;

-

-                public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } }

-                public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } }

-

-                public OptimizedBvhNode LeftChild { get { return _leftChild; } set { _leftChild = value; } }

-                public OptimizedBvhNode RightChild { get { return _rightChild; } set { _rightChild = value; } }

-

-                public int EscapeIndex { get { return _escapeIndex; } set { _escapeIndex = value; } }

-

-                public int SubPart { get { return _subPart; } set { _subPart = value; } }

-                public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = 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
+{
+        /// <summary>
+        /// OptimizedBvhNode contains both internal and leaf node information.
+        /// It hasn't been optimized yet for storage. Some obvious optimizations are:
+        /// Removal of the pointers (can already be done, they are not used for traversal)
+        /// and storing aabbmin/max as quantized integers.
+        /// 'subpart' doesn't need an integer either. It allows to re-use graphics triangle
+        /// meshes stored in a non-uniform way (like batches/subparts of triangle-fans
+        /// </summary>
+        public class OptimizedBvhNode
+        {
+                private Vector3 _aabbMin;
+                private Vector3 _aabbMax;
+
+                //these 2 pointers are obsolete, the stackless traversal just uses the escape index
+                private OptimizedBvhNode _leftChild;
+                private OptimizedBvhNode _rightChild;
+
+                private int _escapeIndex;
+
+                //for child nodes
+                private int _subPart;
+                private int _triangleIndex;
+
+                public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } }
+                public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } }
+
+                public OptimizedBvhNode LeftChild { get { return _leftChild; } set { _leftChild = value; } }
+                public OptimizedBvhNode RightChild { get { return _rightChild; } set { _rightChild = value; } }
+
+                public int EscapeIndex { get { return _escapeIndex; } set { _escapeIndex = value; } }
+
+                public int SubPart { get { return _subPart; } set { _subPart = value; } }
+                public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs
index f61371a..1a911b3 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs
@@ -1,133 +1,133 @@
-/*

-  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 PolyhedralConvexShape : ConvexShape

-        {

-                public PolyhedralConvexShape()

-                {

-                        //m_optionalHull = null;

-                }

-

-                public abstract int VertexCount { get; }

-                public abstract int EdgeCount { get; }

-                public abstract int PlaneCount { get; }

-

-                public abstract void GetEdge(int i, out Vector3 pointA, out Vector3 pointB);

-                public abstract void GetVertex(int i, out Vector3 vertex);

-                public abstract void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i);

-                //      abstract int getIndex(int i); 

-

-                public abstract bool IsInside(Vector3 point, float tolerance);

-

-                // optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp

-                //public class Hull m_optionalHull;

-

-                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)

-                {

-                        Vector3 supVec = new Vector3();

-

-                        float maxDot = -1e30f;

-

-                        float lenSqr = vec.LengthSquared();

-                        if (lenSqr < 0.0001f)

-                        {

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

-                        }

-                        else

-                        {

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

-                                vec *= rlen;

-                        }

-

-                        Vector3 vtx;

-                        float newDot;

-

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

-                        {

-                                GetVertex(i, out vtx);

-                                newDot = Vector3.Dot(vec, vtx);

-                                if (newDot > maxDot)

-                                {

-                                        maxDot = newDot;

-                                        supVec = vtx;

-                                }

-                        }

-                        return supVec;

-                }

-

-                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)

-                {

-                        #warning Think about this

-                        /*Vector3 vtx;

-                        float newDot;

-

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

-                        {

-                                supportVerticesOut[i][3] = -1e30f;

-                        }

-

-                        for (int j = 0; j < vectors.Length; j++)

-                        {

-                                Vector3 vec = vectors[j];

-

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

-                                {

-                                        getVertex(i, out vtx);

-                                        newDot = Vector3.Dot(vec,vtx);

-                                        if (newDot > supportVerticesOut[j][3])

-                                        {

-                                                //WARNING: don't swap next lines, the w component would get overwritten!

-                                                supportVerticesOut[j] = vtx;

-                                                supportVerticesOut[j][3] = newDot;

-                                        }

-                                }

-                        }*/

-                }

-

-                public override void CalculateLocalInertia(float mass, out Vector3 inertia)

-                {

-                        //not yet, return box inertia

-                        float margin = Margin;

-

-                        Matrix ident = Matrix.Identity;

-                        Vector3 aabbMin, aabbMax;

-                        GetAabb(ident, out aabbMin, out aabbMax);

-                        Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f;

-

-                        float lx = 2f * (halfExtents.X + margin);

-                        float ly = 2f * (halfExtents.Y + margin);

-                        float lz = 2f * (halfExtents.Z + margin);

-                        float x2 = lx * lx;

-                        float y2 = ly * ly;

-                        float z2 = lz * lz;

-                        float scaledmass = mass * 0.08333333f;

-

-                        inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2));

-                }

-        }

-}

+/*
+  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 PolyhedralConvexShape : ConvexShape
+        {
+                public PolyhedralConvexShape()
+                {
+                        //m_optionalHull = null;
+                }
+
+                public abstract int VertexCount { get; }
+                public abstract int EdgeCount { get; }
+                public abstract int PlaneCount { get; }
+
+                public abstract void GetEdge(int i, out Vector3 pointA, out Vector3 pointB);
+                public abstract void GetVertex(int i, out Vector3 vertex);
+                public abstract void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i);
+                //      abstract int getIndex(int i); 
+
+                public abstract bool IsInside(Vector3 point, float tolerance);
+
+                // optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp
+                //public class Hull m_optionalHull;
+
+                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
+                {
+                        Vector3 supVec = new Vector3();
+
+                        float maxDot = -1e30f;
+
+                        float lenSqr = vec.LengthSquared();
+                        if (lenSqr < 0.0001f)
+                        {
+                                vec = new Vector3(1, 0, 0);
+                        }
+                        else
+                        {
+                                float rlen = 1f / (float)Math.Sqrt(lenSqr);
+                                vec *= rlen;
+                        }
+
+                        Vector3 vtx;
+                        float newDot;
+
+                        for (int i = 0; i < VertexCount; i++)
+                        {
+                                GetVertex(i, out vtx);
+                                newDot = Vector3.Dot(vec, vtx);
+                                if (newDot > maxDot)
+                                {
+                                        maxDot = newDot;
+                                        supVec = vtx;
+                                }
+                        }
+                        return supVec;
+                }
+
+                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
+                {
+                        #warning Think about this
+                        /*Vector3 vtx;
+                        float newDot;
+
+                        for (int i = 0; i < vectors.Length; i++)
+                        {
+                                supportVerticesOut[i][3] = -1e30f;
+                        }
+
+                        for (int j = 0; j < vectors.Length; j++)
+                        {
+                                Vector3 vec = vectors[j];
+
+                                for (int i = 0; i < getNumVertices(); i++)
+                                {
+                                        getVertex(i, out vtx);
+                                        newDot = Vector3.Dot(vec,vtx);
+                                        if (newDot > supportVerticesOut[j][3])
+                                        {
+                                                //WARNING: don't swap next lines, the w component would get overwritten!
+                                                supportVerticesOut[j] = vtx;
+                                                supportVerticesOut[j][3] = newDot;
+                                        }
+                                }
+                        }*/
+                }
+
+                public override void CalculateLocalInertia(float mass, out Vector3 inertia)
+                {
+                        //not yet, return box inertia
+                        float margin = Margin;
+
+                        Matrix ident = Matrix.Identity;
+                        Vector3 aabbMin, aabbMax;
+                        GetAabb(ident, out aabbMin, out aabbMax);
+                        Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f;
+
+                        float lx = 2f * (halfExtents.X + margin);
+                        float ly = 2f * (halfExtents.Y + margin);
+                        float lz = 2f * (halfExtents.Z + margin);
+                        float x2 = lx * lx;
+                        float y2 = ly * ly;
+                        float z2 = lz * lz;
+                        float scaledmass = mass * 0.08333333f;
+
+                        inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2));
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs
index c660a6f..cc3cea9 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs
@@ -1,116 +1,116 @@
-/*

-  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>

-        /// btSphereShape implements an implicit (getSupportingVertex) Sphere

-        /// </summary>

-        public class SphereShape : ConvexShape

-        {

-                public SphereShape(float radius)

-                        : base()

-                {

-                        Vector3 temp = ImplicitShapeDimensions;

-                        temp.X = radius;

-                        ImplicitShapeDimensions = temp;

-                }

-

-                public float Radius { get { return ImplicitShapeDimensions.X; } }

-

-                public override BroadphaseNativeTypes ShapeType

-                {

-                        get

-                        {

-                                return BroadphaseNativeTypes.Sphere;

-                        }

-                }

-

-                public override string Name

-                {

-                        get

-                        {

-                                return "Sphere";

-                        }

-                }

-

-                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)

-                {

-                        return new Vector3();

-                }

-

-                /// <summary>

-                /// to improve gjk behaviour, use radius+margin as the full margin, so never get into the penetration case

-                /// this means, non-uniform scaling is not supported anymore

-                /// </summary>

-                public override float Margin

-                {

-                        get

-                        {

-                                return LocalScaling.X * Radius + base.Margin;

-                        }

-                        set

-                        {

-                                base.Margin = value;

-                        }

-                }

-

-                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)

-                {

-                        if (supportVerticesOut != null)

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

-                                        supportVerticesOut[i] = new Vector3();

-                }

-

-                public override void CalculateLocalInertia(float mass, out Vector3 inertia)

-                {

-                        float elem = 0.4f * mass * Margin * Margin;

-                        inertia = new Vector3(elem, elem, elem);

-                }

-

-                public override Vector3 LocalGetSupportingVertex(Vector3 vec)

-                {

-                        Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec);

-

-                        Vector3 vecnorm = vec;

-                        if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))

-                        {

-                                vecnorm = new Vector3(-1f, -1f, -1f);

-                        }

-                        vecnorm.Normalize();

-                        supVertex += Margin * vecnorm;

-                        return supVertex;

-                }

-

-                public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)

-                {

-                        Vector3 center = t.Translation;

-                        Vector3 extent = new Vector3(Margin, Margin, Margin);

-                        aabbMin = center - extent;

-                        aabbMax = center + extent;

-                }

-        }

-}

+/*
+  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>
+        /// btSphereShape implements an implicit (getSupportingVertex) Sphere
+        /// </summary>
+        public class SphereShape : ConvexShape
+        {
+                public SphereShape(float radius)
+                        : base()
+                {
+                        Vector3 temp = ImplicitShapeDimensions;
+                        temp.X = radius;
+                        ImplicitShapeDimensions = temp;
+                }
+
+                public float Radius { get { return ImplicitShapeDimensions.X; } }
+
+                public override BroadphaseNativeTypes ShapeType
+                {
+                        get
+                        {
+                                return BroadphaseNativeTypes.Sphere;
+                        }
+                }
+
+                public override string Name
+                {
+                        get
+                        {
+                                return "Sphere";
+                        }
+                }
+
+                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
+                {
+                        return new Vector3();
+                }
+
+                /// <summary>
+                /// to improve gjk behaviour, use radius+margin as the full margin, so never get into the penetration case
+                /// this means, non-uniform scaling is not supported anymore
+                /// </summary>
+                public override float Margin
+                {
+                        get
+                        {
+                                return LocalScaling.X * Radius + base.Margin;
+                        }
+                        set
+                        {
+                                base.Margin = value;
+                        }
+                }
+
+                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
+                {
+                        if (supportVerticesOut != null)
+                                for (int i = 0; i < supportVerticesOut.Length; i++)
+                                        supportVerticesOut[i] = new Vector3();
+                }
+
+                public override void CalculateLocalInertia(float mass, out Vector3 inertia)
+                {
+                        float elem = 0.4f * mass * Margin * Margin;
+                        inertia = new Vector3(elem, elem, elem);
+                }
+
+                public override Vector3 LocalGetSupportingVertex(Vector3 vec)
+                {
+                        Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec);
+
+                        Vector3 vecnorm = vec;
+                        if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))
+                        {
+                                vecnorm = new Vector3(-1f, -1f, -1f);
+                        }
+                        vecnorm.Normalize();
+                        supVertex += Margin * vecnorm;
+                        return supVertex;
+                }
+
+                public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
+                {
+                        Vector3 center = t.Translation;
+                        Vector3 extent = new Vector3(Margin, Margin, Margin);
+                        aabbMin = center - extent;
+                        aabbMax = center + extent;
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs
index 49604a0..a79a145 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs
@@ -1,124 +1,124 @@
-/*

-  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 StaticPlaneShape : ConcaveShape

-        {

-                private Vector3 _localAabbMin;

-                private Vector3 _localAabbMax;

-

-                private Vector3 _planeNormal;

-                private float _planeConstant;

-                private Vector3 _localScaling;

-

-                public StaticPlaneShape(Vector3 planeNormal, float planeConstant)

-                {

-                        _planeNormal = planeNormal;

-                        _planeConstant = planeConstant;

-                        _localScaling = new Vector3();

-                }

-

-                protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } }

-                protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } }

-

-                protected Vector3 PlaneNormal { get { return _planeNormal; } set { _planeNormal = value; } }

-                protected float PlaneConstant { get { return _planeConstant; } set { _planeConstant = value; } }

-

-                public override BroadphaseNativeTypes ShapeType

-                {

-                        get

-                        {

-                                return BroadphaseNativeTypes.StaticPlane;

-                        }

-                }

-

-                public override Vector3 LocalScaling

-                {

-                        get

-                        {

-                                return _localScaling;

-                        }

-                        set

-                        {

-                                _localScaling = value;

-                        }

-                }

-

-                public override string Name

-                {

-                        get

-                        {

-                                return "StaticPlane";

-                        }

-                }

-

-                public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)

-                {

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

-

-                        Vector3 center = _planeNormal * _planeConstant;

-                        aabbMin = center + infvec * _planeNormal;

-                        aabbMax = aabbMin;

-                        MathHelper.SetMin(ref aabbMin, center - infvec * _planeNormal);

-                        MathHelper.SetMax(ref aabbMax, center - infvec * _planeNormal);

-

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

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

-                }

-

-                public override void CalculateLocalInertia(float mass, out Vector3 inertia)

-                {

-                        //moving concave objects not supported

-                        inertia = new Vector3();

-                }

-

-        public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) {

-            Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f;

-            float radius = halfExtents.Length();

-            Vector3 center = (aabbMax + aabbMin) * 0.5f;

-

-            //this is where the triangles are generated, given AABB and plane equation (normal/constant)

-            Vector3 tangentDir0 = new Vector3(), tangentDir1 = new Vector3();

-

-            //tangentDir0/tangentDir1 can be precalculated

-            MathHelper.PlaneSpace1(_planeNormal, ref tangentDir0, ref tangentDir1);

-

-            Vector3 projectedCenter = center - (Vector3.Dot(_planeNormal, center) - _planeConstant) * _planeNormal;

-

-            Vector3[] triangle = new Vector3[3];

-            triangle[0] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius;

-            triangle[1] = projectedCenter + tangentDir0 * radius - tangentDir1 * radius;

-            triangle[2] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius;

-            callback.ProcessTriangle(triangle, 0, 0);

-

-            triangle[0] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius;

-            triangle[1] = projectedCenter - tangentDir0 * radius + tangentDir1 * radius;

-            triangle[2] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius;

-            callback.ProcessTriangle(triangle, 0, 1);

-        }

-    }

-}

+/*
+  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 StaticPlaneShape : ConcaveShape
+        {
+                private Vector3 _localAabbMin;
+                private Vector3 _localAabbMax;
+
+                private Vector3 _planeNormal;
+                private float _planeConstant;
+                private Vector3 _localScaling;
+
+                public StaticPlaneShape(Vector3 planeNormal, float planeConstant)
+                {
+                        _planeNormal = planeNormal;
+                        _planeConstant = planeConstant;
+                        _localScaling = new Vector3();
+                }
+
+                protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } }
+                protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } }
+
+                protected Vector3 PlaneNormal { get { return _planeNormal; } set { _planeNormal = value; } }
+                protected float PlaneConstant { get { return _planeConstant; } set { _planeConstant = value; } }
+
+                public override BroadphaseNativeTypes ShapeType
+                {
+                        get
+                        {
+                                return BroadphaseNativeTypes.StaticPlane;
+                        }
+                }
+
+                public override Vector3 LocalScaling
+                {
+                        get
+                        {
+                                return _localScaling;
+                        }
+                        set
+                        {
+                                _localScaling = value;
+                        }
+                }
+
+                public override string Name
+                {
+                        get
+                        {
+                                return "StaticPlane";
+                        }
+                }
+
+                public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
+                {
+                        Vector3 infvec = new Vector3(1e30f, 1e30f, 1e30f);
+
+                        Vector3 center = _planeNormal * _planeConstant;
+                        aabbMin = center + infvec * _planeNormal;
+                        aabbMax = aabbMin;
+                        MathHelper.SetMin(ref aabbMin, center - infvec * _planeNormal);
+                        MathHelper.SetMax(ref aabbMax, center - infvec * _planeNormal);
+
+                        aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
+                        aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
+                }
+
+                public override void CalculateLocalInertia(float mass, out Vector3 inertia)
+                {
+                        //moving concave objects not supported
+                        inertia = new Vector3();
+                }
+
+        public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) {
+            Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f;
+            float radius = halfExtents.Length();
+            Vector3 center = (aabbMax + aabbMin) * 0.5f;
+
+            //this is where the triangles are generated, given AABB and plane equation (normal/constant)
+            Vector3 tangentDir0 = new Vector3(), tangentDir1 = new Vector3();
+
+            //tangentDir0/tangentDir1 can be precalculated
+            MathHelper.PlaneSpace1(_planeNormal, ref tangentDir0, ref tangentDir1);
+
+            Vector3 projectedCenter = center - (Vector3.Dot(_planeNormal, center) - _planeConstant) * _planeNormal;
+
+            Vector3[] triangle = new Vector3[3];
+            triangle[0] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius;
+            triangle[1] = projectedCenter + tangentDir0 * radius - tangentDir1 * radius;
+            triangle[2] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius;
+            callback.ProcessTriangle(triangle, 0, 0);
+
+            triangle[0] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius;
+            triangle[1] = projectedCenter - tangentDir0 * radius + tangentDir1 * radius;
+            triangle[2] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius;
+            callback.ProcessTriangle(triangle, 0, 1);
+        }
+    }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs
index 2a2ce27..20baccb 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.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

-{

-        /// <summary>

-        /// PHY_ScalarType enumerates possible scalar types.

-        /// See the StridingMeshInterface for its use

-        /// </summary>

-        public enum PHY_ScalarType

-        {

-                PHY_FLOAT,

-                PHY_DOUBLE,

-                PHY_INTEGER,

-                PHY_SHORT,

-                PHY_FIXEDPOINT88

-        }

-

-        /// <summary>

-        /// StridingMeshInterface is the interface class for high performance access to triangle meshes

-        /// It allows for sharing graphics and collision meshes. Also it provides locking/unlocking of graphics meshes that are in gpu memory.

-        /// </summary>

-        public abstract class StridingMeshInterface

-        {

-                protected Vector3 _scaling;

-

-                public StridingMeshInterface()

-                {

-                        _scaling = new Vector3(1f,1f,1f);

-                }

-

-                public void InternalProcessAllTriangles(ITriangleIndexCallback callback, Vector3 aabbMin, Vector3 aabbMax)

-                {

-                        int numtotalphysicsverts = 0;

-            int numtriangles, gfxindex;

-                        int part, graphicssubparts = SubPartsCount();

-            Vector3[] triangle = new Vector3[3];

-            List<Vector3> verts;

-            List<int> indicies;

-

-                        Vector3 meshScaling = Scaling;

-

-                        //if the number of parts is big, the performance might drop due to the innerloop switch on indextype

-                        for (part = 0; part < graphicssubparts; part++)

-                        {

-                GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, part);

-                            numtotalphysicsverts += numtriangles * 3; //upper bound

-

-                for (gfxindex = 0; gfxindex < numtriangles; gfxindex++)

-                {

-                    triangle[0] = verts[indicies[gfxindex * 3 + 0]];

-                    triangle[1] = verts[indicies[gfxindex * 3 + 1]];

-                    triangle[2] = verts[indicies[gfxindex * 3 + 2]];

-

-                    callback.ProcessTriangleIndex(triangle, part, gfxindex);

-                }

-

-                            UnLockReadOnlyVertexBase(part);

-                        }

-                }

-

-

-                // get read and write access to a subpart of a triangle mesh

-                // this subpart has a continuous array of vertices and indices

-                // in this way the mesh can be handled as chunks of memory with striding

-                // very similar to OpenGL vertexarray support

-                // make a call to unLockVertexBase when the read and write access is finished   

-                public abstract void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart);

-

-        public abstract void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart);

-        

-                // unLockVertexBase finishes the access to a subpart of the triangle mesh

-                // make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished

-                public abstract void UnLockVertexBase(int subpart);

-

-                public abstract void UnLockReadOnlyVertexBase(int subpart);

-

-

-                // getNumSubParts returns the number of seperate subparts

-                // each subpart has a continuous array of vertices and indices

-        public abstract int SubPartsCount();

-

-                public abstract void PreallocateVertices(int numverts);

-                public abstract void PreallocateIndices(int numindices);

-

-                public Vector3 Scaling

-        {           

-                        get { return _scaling; }

-            set { _scaling = 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
+{
+        /// <summary>
+        /// PHY_ScalarType enumerates possible scalar types.
+        /// See the StridingMeshInterface for its use
+        /// </summary>
+        public enum PHY_ScalarType
+        {
+                PHY_FLOAT,
+                PHY_DOUBLE,
+                PHY_INTEGER,
+                PHY_SHORT,
+                PHY_FIXEDPOINT88
+        }
+
+        /// <summary>
+        /// StridingMeshInterface is the interface class for high performance access to triangle meshes
+        /// It allows for sharing graphics and collision meshes. Also it provides locking/unlocking of graphics meshes that are in gpu memory.
+        /// </summary>
+        public abstract class StridingMeshInterface
+        {
+                protected Vector3 _scaling;
+
+                public StridingMeshInterface()
+                {
+                        _scaling = new Vector3(1f,1f,1f);
+                }
+
+                public void InternalProcessAllTriangles(ITriangleIndexCallback callback, Vector3 aabbMin, Vector3 aabbMax)
+                {
+                        int numtotalphysicsverts = 0;
+            int numtriangles, gfxindex;
+                        int part, graphicssubparts = SubPartsCount();
+            Vector3[] triangle = new Vector3[3];
+            List<Vector3> verts;
+            List<int> indicies;
+
+                        Vector3 meshScaling = Scaling;
+
+                        //if the number of parts is big, the performance might drop due to the innerloop switch on indextype
+                        for (part = 0; part < graphicssubparts; part++)
+                        {
+                GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, part);
+                            numtotalphysicsverts += numtriangles * 3; //upper bound
+
+                for (gfxindex = 0; gfxindex < numtriangles; gfxindex++)
+                {
+                    triangle[0] = verts[indicies[gfxindex * 3 + 0]];
+                    triangle[1] = verts[indicies[gfxindex * 3 + 1]];
+                    triangle[2] = verts[indicies[gfxindex * 3 + 2]];
+
+                    callback.ProcessTriangleIndex(triangle, part, gfxindex);
+                }
+
+                            UnLockReadOnlyVertexBase(part);
+                        }
+                }
+
+
+                // get read and write access to a subpart of a triangle mesh
+                // this subpart has a continuous array of vertices and indices
+                // in this way the mesh can be handled as chunks of memory with striding
+                // very similar to OpenGL vertexarray support
+                // make a call to unLockVertexBase when the read and write access is finished   
+                public abstract void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart);
+
+        public abstract void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart);
+        
+                // unLockVertexBase finishes the access to a subpart of the triangle mesh
+                // make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished
+                public abstract void UnLockVertexBase(int subpart);
+
+                public abstract void UnLockReadOnlyVertexBase(int subpart);
+
+
+                // getNumSubParts returns the number of seperate subparts
+                // each subpart has a continuous array of vertices and indices
+        public abstract int SubPartsCount();
+
+                public abstract void PreallocateVertices(int numverts);
+                public abstract void PreallocateIndices(int numindices);
+
+                public Vector3 Scaling
+        {           
+                        get { return _scaling; }
+            set { _scaling = value; }
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs
index ffb5813..eab822e 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs
@@ -1,67 +1,67 @@
-/*

-  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

-{

-        internal class SupportVertexCallback : ITriangleCallback

-        {

-                private Vector3 _supportVertexLocal;

-

-                private Matrix _worldTransform;

-                private float _maxDot;

-                private Vector3 _supportVecLocal;

-

-                public SupportVertexCallback(Vector3 supportVecWorld, Matrix trans)

-                {

-                        _supportVertexLocal = new Vector3();

-                        _worldTransform = trans;

-                        _maxDot = -1e30f;

-                        _supportVecLocal = Vector3.TransformNormal(supportVecWorld, _worldTransform);

-                }

-

-                public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } }

-                public float MaxDot { get { return _maxDot; } set { _maxDot = value; } }

-                public Vector3 SupportVectorLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } }

-

-                public Vector3 SupportVertexLocal { get { return _supportVertexLocal; } }

-                public Vector3 SupportVertexWorldSpace { get { return MathHelper.MatrixToVector(_worldTransform, _supportVertexLocal); } }

-

-                #region ITriangleCallback Members

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

-                {

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

-                        {

-                                float dot = Vector3.Dot(_supportVecLocal, triangle[i]);

-                                if (dot > _maxDot)

-                                {

-                                        _maxDot = dot;

-                                        _supportVertexLocal = triangle[i];

-                                }

-                        }

-                }

-                #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
+{
+        internal class SupportVertexCallback : ITriangleCallback
+        {
+                private Vector3 _supportVertexLocal;
+
+                private Matrix _worldTransform;
+                private float _maxDot;
+                private Vector3 _supportVecLocal;
+
+                public SupportVertexCallback(Vector3 supportVecWorld, Matrix trans)
+                {
+                        _supportVertexLocal = new Vector3();
+                        _worldTransform = trans;
+                        _maxDot = -1e30f;
+                        _supportVecLocal = Vector3.TransformNormal(supportVecWorld, _worldTransform);
+                }
+
+                public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } }
+                public float MaxDot { get { return _maxDot; } set { _maxDot = value; } }
+                public Vector3 SupportVectorLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } }
+
+                public Vector3 SupportVertexLocal { get { return _supportVertexLocal; } }
+                public Vector3 SupportVertexWorldSpace { get { return MathHelper.MatrixToVector(_worldTransform, _supportVertexLocal); } }
+
+                #region ITriangleCallback Members
+                public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex)
+                {
+                        for (int i = 0; i < 3; i++)
+                        {
+                                float dot = Vector3.Dot(_supportVecLocal, triangle[i]);
+                                if (dot > _maxDot)
+                                {
+                                        _maxDot = dot;
+                                        _supportVertexLocal = triangle[i];
+                                }
+                        }
+                }
+                #endregion
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs
index 158e42a..a472583 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs
@@ -1,80 +1,80 @@
-/*

-  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 Triangle

-        {

-                private Vector3 _vertexA;

-                private Vector3 _vertexB;

-                private Vector3 _vertexC;

-                private int _partId;

-                private int _triangleIndex;

-

-                public Vector3 VertexA { get { return _vertexA; } set { _vertexA = value; } }

-                public Vector3 VertexB { get { return _vertexB; } set { _vertexB = value; } }

-                public Vector3 VertexC { get { return _vertexC; } set { _vertexC = value; } }

-                public int PartId { get { return _partId; } set { _partId = value; } }

-                public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } }

-        }

-

-        /// <summary>

-        /// example usage of this class:

-        ///                     TriangleBuffer triBuf;

-        ///                     concaveShape.processAllTriangles(triBuf, out aabbMin, out aabbMax);

-        ///                     for (int i = 0; i < triBuf.getNumTriangles(); i++)

-        ///                     {

-        ///                             Triangle tri = triBuf.getTriangle(i);

-        ///                             //do something useful here with the triangle

-        ///                     }

-        /// </summary>

-        public class TriangleBuffer : ITriangleCallback

-        {

-                private List<Triangle> _triangleBuffer = new List<Triangle>();

-

-                public int TriangleCount { get { return _triangleBuffer.Count; } }

-                public Triangle this[int index] { get { return _triangleBuffer[index]; } }

-

-                public void ClearBuffer()

-                {

-                        _triangleBuffer.Clear();

-                }

-

-                #region ITriangleCallback Members

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

-                {

-                        Triangle tri = new Triangle();

-                        tri.VertexA = triangle[0];

-                        tri.VertexB = triangle[1];

-                        tri.VertexC = triangle[2];

-                        tri.PartId = partID;

-                        tri.TriangleIndex = triangleIndex;

-

-                        _triangleBuffer.Add(tri);

-                }

-                #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 class Triangle
+        {
+                private Vector3 _vertexA;
+                private Vector3 _vertexB;
+                private Vector3 _vertexC;
+                private int _partId;
+                private int _triangleIndex;
+
+                public Vector3 VertexA { get { return _vertexA; } set { _vertexA = value; } }
+                public Vector3 VertexB { get { return _vertexB; } set { _vertexB = value; } }
+                public Vector3 VertexC { get { return _vertexC; } set { _vertexC = value; } }
+                public int PartId { get { return _partId; } set { _partId = value; } }
+                public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } }
+        }
+
+        /// <summary>
+        /// example usage of this class:
+        ///                     TriangleBuffer triBuf;
+        ///                     concaveShape.processAllTriangles(triBuf, out aabbMin, out aabbMax);
+        ///                     for (int i = 0; i < triBuf.getNumTriangles(); i++)
+        ///                     {
+        ///                             Triangle tri = triBuf.getTriangle(i);
+        ///                             //do something useful here with the triangle
+        ///                     }
+        /// </summary>
+        public class TriangleBuffer : ITriangleCallback
+        {
+                private List<Triangle> _triangleBuffer = new List<Triangle>();
+
+                public int TriangleCount { get { return _triangleBuffer.Count; } }
+                public Triangle this[int index] { get { return _triangleBuffer[index]; } }
+
+                public void ClearBuffer()
+                {
+                        _triangleBuffer.Clear();
+                }
+
+                #region ITriangleCallback Members
+                public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex)
+                {
+                        Triangle tri = new Triangle();
+                        tri.VertexA = triangle[0];
+                        tri.VertexB = triangle[1];
+                        tri.VertexC = triangle[2];
+                        tri.PartId = partID;
+                        tri.TriangleIndex = triangleIndex;
+
+                        _triangleBuffer.Add(tri);
+                }
+                #endregion
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs
index ac892c2..938765b 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs
@@ -1,33 +1,33 @@
-/*

-  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 interface ITriangleCallback

-    {

-                void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex);

-    }

-}

+/*
+  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 interface ITriangleCallback
+    {
+                void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex);
+    }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs
index 6d0d7b5..52c08ed 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs
@@ -1,136 +1,136 @@
-/*

-  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>

-        /// IndexedMesh indexes into existing vertex and index arrays, in a similar way OpenGL glDrawElements

-        /// instead of the number of indices, we pass the number of triangles

-        /// </summary>

-        public struct IndexedMesh

-        {

-                private int _numTriangles;

-                private int[] _triangleIndexBase;

-                private int _triangleIndexStride;

-                private int _numVertices;

-                private Vector3[] _vertexBase;

-                private int _vertexStride;

-

-                public IndexedMesh(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride)

-                {

-                        _numTriangles = numTriangleIndices;

-                        _triangleIndexBase = triangleIndexBase;

-                        _triangleIndexStride = triangleIndexStride;

-                        _vertexBase = vertexBase;

-                        _numVertices = numVertices;

-                        _vertexStride = vertexStride;

-                }

-

-                public IndexedMesh(int[] triangleIndexBase, Vector3[] vertexBase)

-                {

-                        _numTriangles = triangleIndexBase.Length;

-                        _triangleIndexBase = triangleIndexBase;

-                        _triangleIndexStride = 32;

-                        _vertexBase = vertexBase;

-                        _numVertices = vertexBase.Length;

-                        _vertexStride = 24;

-                }

-

-                public int TriangleCount { get { return _numTriangles; } set { _numTriangles = value; } }

-                public int[] TriangleIndexBase { get { return _triangleIndexBase; } set { _triangleIndexBase = value; } }

-                public int TriangleIndexStride { get { return _triangleIndexStride; } set { _triangleIndexStride = value; } }

-                public int VertexCount { get { return _numVertices; } set { _numVertices = value; } }

-                public Vector3[] VertexBase { get { return _vertexBase; } set { _vertexBase = value; } }

-                public int VertexStride { get { return _vertexStride; } set { _vertexStride = value; } }

-        }

-

-        /// <summary>

-        /// TriangleIndexVertexArray allows to use multiple meshes, by indexing into existing triangle/index arrays.

-        /// Additional meshes can be added using addIndexedMesh

-        /// </summary>

-        public class TriangleIndexVertexArray : StridingMeshInterface

-        {

-                List<IndexedMesh> _indexedMeshes = new List<IndexedMesh>();

-

-                public TriangleIndexVertexArray() { }

-

-                public TriangleIndexVertexArray(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride)

-                {

-                        IndexedMesh mesh = new IndexedMesh();

-                        mesh.TriangleCount = numTriangleIndices;

-                        mesh.TriangleIndexBase = triangleIndexBase;

-                        mesh.TriangleIndexStride = triangleIndexStride;

-                        mesh.VertexBase = vertexBase;

-                        mesh.VertexCount = numVertices;

-                        mesh.VertexStride = vertexStride;

-

-                        AddIndexedMesh(mesh);

-                }

-

-                public TriangleIndexVertexArray(int[] triangleIndexBase, Vector3[] vertexBase)

-                        : this(triangleIndexBase.Length, triangleIndexBase, 32, vertexBase.Length, vertexBase, 24) { }

-

-                public void AddIndexedMesh(IndexedMesh indexedMesh)

-                {

-                        _indexedMeshes.Add(indexedMesh);

-                }

-

-                public override void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart)

-                {

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

-                }

-

-                public override void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart)

-                {

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

-                }

-

-                public override void UnLockVertexBase(int subpart)

-                {

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

-                }

-

-                public override void UnLockReadOnlyVertexBase(int subpart)

-                {

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

-                }

-

-                public override int SubPartsCount()

-                {

-                        return _indexedMeshes.Count;

-                }

-

-                public override void PreallocateVertices(int numverts)

-                {

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

-                }

-

-                public override void PreallocateIndices(int numindices)

-                {

-                        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
+{
+        /// <summary>
+        /// IndexedMesh indexes into existing vertex and index arrays, in a similar way OpenGL glDrawElements
+        /// instead of the number of indices, we pass the number of triangles
+        /// </summary>
+        public struct IndexedMesh
+        {
+                private int _numTriangles;
+                private int[] _triangleIndexBase;
+                private int _triangleIndexStride;
+                private int _numVertices;
+                private Vector3[] _vertexBase;
+                private int _vertexStride;
+
+                public IndexedMesh(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride)
+                {
+                        _numTriangles = numTriangleIndices;
+                        _triangleIndexBase = triangleIndexBase;
+                        _triangleIndexStride = triangleIndexStride;
+                        _vertexBase = vertexBase;
+                        _numVertices = numVertices;
+                        _vertexStride = vertexStride;
+                }
+
+                public IndexedMesh(int[] triangleIndexBase, Vector3[] vertexBase)
+                {
+                        _numTriangles = triangleIndexBase.Length;
+                        _triangleIndexBase = triangleIndexBase;
+                        _triangleIndexStride = 32;
+                        _vertexBase = vertexBase;
+                        _numVertices = vertexBase.Length;
+                        _vertexStride = 24;
+                }
+
+                public int TriangleCount { get { return _numTriangles; } set { _numTriangles = value; } }
+                public int[] TriangleIndexBase { get { return _triangleIndexBase; } set { _triangleIndexBase = value; } }
+                public int TriangleIndexStride { get { return _triangleIndexStride; } set { _triangleIndexStride = value; } }
+                public int VertexCount { get { return _numVertices; } set { _numVertices = value; } }
+                public Vector3[] VertexBase { get { return _vertexBase; } set { _vertexBase = value; } }
+                public int VertexStride { get { return _vertexStride; } set { _vertexStride = value; } }
+        }
+
+        /// <summary>
+        /// TriangleIndexVertexArray allows to use multiple meshes, by indexing into existing triangle/index arrays.
+        /// Additional meshes can be added using addIndexedMesh
+        /// </summary>
+        public class TriangleIndexVertexArray : StridingMeshInterface
+        {
+                List<IndexedMesh> _indexedMeshes = new List<IndexedMesh>();
+
+                public TriangleIndexVertexArray() { }
+
+                public TriangleIndexVertexArray(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride)
+                {
+                        IndexedMesh mesh = new IndexedMesh();
+                        mesh.TriangleCount = numTriangleIndices;
+                        mesh.TriangleIndexBase = triangleIndexBase;
+                        mesh.TriangleIndexStride = triangleIndexStride;
+                        mesh.VertexBase = vertexBase;
+                        mesh.VertexCount = numVertices;
+                        mesh.VertexStride = vertexStride;
+
+                        AddIndexedMesh(mesh);
+                }
+
+                public TriangleIndexVertexArray(int[] triangleIndexBase, Vector3[] vertexBase)
+                        : this(triangleIndexBase.Length, triangleIndexBase, 32, vertexBase.Length, vertexBase, 24) { }
+
+                public void AddIndexedMesh(IndexedMesh indexedMesh)
+                {
+                        _indexedMeshes.Add(indexedMesh);
+                }
+
+                public override void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart)
+                {
+                        throw new Exception("The method or operation is not implemented.");
+                }
+
+                public override void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart)
+                {
+                        throw new Exception("The method or operation is not implemented.");
+                }
+
+                public override void UnLockVertexBase(int subpart)
+                {
+                        throw new Exception("The method or operation is not implemented.");
+                }
+
+                public override void UnLockReadOnlyVertexBase(int subpart)
+                {
+                        throw new Exception("The method or operation is not implemented.");
+                }
+
+                public override int SubPartsCount()
+                {
+                        return _indexedMeshes.Count;
+                }
+
+                public override void PreallocateVertices(int numverts)
+                {
+                        throw new Exception("The method or operation is not implemented.");
+                }
+
+                public override void PreallocateIndices(int numindices)
+                {
+                        throw new Exception("The method or operation is not implemented.");
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs
index 1426355..d5b93ba 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs
@@ -1,102 +1,102 @@
-/*

-  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

-{

-    class TriangleMesh : StridingMeshInterface

-    {

-        int _numTriangles;

-        List<Vector3> _verts;

-

-        public TriangleMesh()

-        {

-            _numTriangles = 0;

-            _verts = new List<Vector3>();

-        }

-

-        void AddTriangle(Vector3 vertex0, Vector3 vertex1, Vector3 vertex2)

-        {

-            _verts.Add(vertex0);

-            _verts.Add(vertex1);

-            _verts.Add(vertex2);

-            _numTriangles++;

-        }

-

-        public override void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart)

-        {

-            verts = new List<Vector3>();

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

-            {

-                verts.Add(_verts[subpart * 3 + i]);

-            }

-            indicies = new List<int>();

-            indicies.Add(0);

-            indicies.Add(1);

-            indicies.Add(2);

-            numfaces = 1;

-        }

-

-        public override void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart)

-        {

-            verts = new List<Vector3>();

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

-            {

-                verts.Add(_verts[subpart * 3 + i]);

-            }

-            indicies = new List<int>();

-            indicies.Add(0);

-            indicies.Add(1);

-            indicies.Add(2);

-            numfaces = 1;

-        }

-

-        public override void UnLockVertexBase(int subpart)

-        {

-

-        }

-

-        public override void UnLockReadOnlyVertexBase(int subpart)

-        {

-

-        }

-

-        public override int SubPartsCount()

-        {

-            return _numTriangles;

-        }

-

-        public override void PreallocateVertices(int numverts)

-        {

-

-        }

-

-        public override void PreallocateIndices(int numindices)

-        {

-

-        }

-    }

-}

+/*
+  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
+{
+    class TriangleMesh : StridingMeshInterface
+    {
+        int _numTriangles;
+        List<Vector3> _verts;
+
+        public TriangleMesh()
+        {
+            _numTriangles = 0;
+            _verts = new List<Vector3>();
+        }
+
+        void AddTriangle(Vector3 vertex0, Vector3 vertex1, Vector3 vertex2)
+        {
+            _verts.Add(vertex0);
+            _verts.Add(vertex1);
+            _verts.Add(vertex2);
+            _numTriangles++;
+        }
+
+        public override void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart)
+        {
+            verts = new List<Vector3>();
+            for (int i = 0; i < 3; i++)
+            {
+                verts.Add(_verts[subpart * 3 + i]);
+            }
+            indicies = new List<int>();
+            indicies.Add(0);
+            indicies.Add(1);
+            indicies.Add(2);
+            numfaces = 1;
+        }
+
+        public override void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart)
+        {
+            verts = new List<Vector3>();
+            for (int i = 0; i < 3; i++)
+            {
+                verts.Add(_verts[subpart * 3 + i]);
+            }
+            indicies = new List<int>();
+            indicies.Add(0);
+            indicies.Add(1);
+            indicies.Add(2);
+            numfaces = 1;
+        }
+
+        public override void UnLockVertexBase(int subpart)
+        {
+
+        }
+
+        public override void UnLockReadOnlyVertexBase(int subpart)
+        {
+
+        }
+
+        public override int SubPartsCount()
+        {
+            return _numTriangles;
+        }
+
+        public override void PreallocateVertices(int numverts)
+        {
+
+        }
+
+        public override void PreallocateIndices(int numindices)
+        {
+
+        }
+    }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs
index bc8f09e..5f5a0cf 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs
@@ -1,160 +1,160 @@
-/*

-  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>

-        /// Concave triangle mesh. Uses an interface to access the triangles to allow for sharing graphics/physics triangles.

-        /// </summary>

-        public class TriangleMeshShape : ConcaveShape

-        {

-                private StridingMeshInterface _meshInterface;

-                private Vector3 _localAabbMin;

-                private Vector3 _localAabbMax;

-

-                public TriangleMeshShape(StridingMeshInterface meshInterface)

-                {

-                        this._meshInterface = meshInterface;

-                        RecalcLocalAabb();

-                }

-

-                protected StridingMeshInterface MeshInterface { get { return _meshInterface; } set { _meshInterface = value; } }

-                protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } }

-                protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } }

-

-                public override BroadphaseNativeTypes ShapeType

-                {

-                        get

-                        {

-                                return BroadphaseNativeTypes.TriangleMesh;

-                        }

-                }

-

-                public override Vector3 LocalScaling

-                {

-                        get

-                        {

-                                return _meshInterface.Scaling;

-                        }

-                        set

-                        {

-                                _meshInterface.Scaling = value;

-                        }

-                }

-

-                public override string Name

-                {

-                        get

-                        {

-                                return "TriangleMesh";

-                        }

-                }

-

-                public void RecalcLocalAabb()

-                {

-                        {

-                                Vector3 vec = new Vector3();

-                                vec.X = 1f;

-                                Vector3 tmp = LocalGetSupportingVertex(vec);

-                                _localAabbMax.X = tmp.X + CollisionMargin;

-                                vec.X = -1f;

-                                tmp = LocalGetSupportingVertex(vec);

-                                _localAabbMin.X = tmp.X - CollisionMargin;

-                        }

-                        {

-                                Vector3 vec = new Vector3();

-                                vec.Y = 1f;

-                                Vector3 tmp = LocalGetSupportingVertex(vec);

-                                _localAabbMax.Y = tmp.Y + CollisionMargin;

-                                vec.Y = -1f;

-                                tmp = LocalGetSupportingVertex(vec);

-                                _localAabbMin.Y = tmp.Y - CollisionMargin;

-                        }

-                        {

-                                Vector3 vec = new Vector3();

-                                vec.Z = 1f;

-                                Vector3 tmp = LocalGetSupportingVertex(vec);

-                                _localAabbMax.Z = tmp.Z + CollisionMargin;

-                                vec.Z = -1f;

-                                tmp = LocalGetSupportingVertex(vec);

-                                _localAabbMin.Z = tmp.Z - CollisionMargin;

-                        }

-                }

-

-                public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)

-                {

-            LocalProcessAllTriangles(callback, aabbMax, aabbMax);

-                }

-

-        protected void LocalProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)

-        {

-            FilteredCallback filterCallback = new FilteredCallback(callback, aabbMin, aabbMax);

-            _meshInterface.InternalProcessAllTriangles(filterCallback, aabbMin, aabbMax);

-        }

-

-        public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)

-                {

-                        Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin);

-                        Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin);

-

-                        Matrix abs_b = MathHelper.Absolute(t);

-

-                        Vector3 center = MathHelper.MatrixToVector(t, localCenter);

-

-                        Vector3 extent = new Vector3(Vector3.Dot(new Vector3(abs_b.M11, abs_b.M12, abs_b.M13), localHalfExtents),

-                                   Vector3.Dot(new Vector3(abs_b.M21, abs_b.M22, abs_b.M23), localHalfExtents),

-                                  Vector3.Dot(new Vector3(abs_b.M31, abs_b.M32, abs_b.M33), localHalfExtents));

-                        extent += new Vector3(Margin, Margin, Margin);

-

-                        aabbMin = center - extent;

-                        aabbMax = center + extent;

-                }

-

-                public override void CalculateLocalInertia(float mass, out Vector3 inertia)

-                {

-                        inertia = new Vector3();

-                        //moving concave objects not supported

-                        BulletDebug.Assert(false);

-                }

-

-                public virtual Vector3 LocalGetSupportingVertex(Vector3 vec)

-                {

-                        Vector3 supportVertex;

-                        Matrix ident = Matrix.Identity;

-                        SupportVertexCallback supportCallback = new SupportVertexCallback(vec, ident);

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

-                        LocalProcessAllTriangles(supportCallback, -aabbMax, aabbMax);

-                        supportVertex = supportCallback.SupportVertexLocal;

-                        return supportVertex;

-                }

-

-                public virtual Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)

-                {

-                        BulletDebug.Assert(false);

-                        return LocalGetSupportingVertex(vec);

-                }

-        }

-}

+/*
+  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>
+        /// Concave triangle mesh. Uses an interface to access the triangles to allow for sharing graphics/physics triangles.
+        /// </summary>
+        public class TriangleMeshShape : ConcaveShape
+        {
+                private StridingMeshInterface _meshInterface;
+                private Vector3 _localAabbMin;
+                private Vector3 _localAabbMax;
+
+                public TriangleMeshShape(StridingMeshInterface meshInterface)
+                {
+                        this._meshInterface = meshInterface;
+                        RecalcLocalAabb();
+                }
+
+                protected StridingMeshInterface MeshInterface { get { return _meshInterface; } set { _meshInterface = value; } }
+                protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } }
+                protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } }
+
+                public override BroadphaseNativeTypes ShapeType
+                {
+                        get
+                        {
+                                return BroadphaseNativeTypes.TriangleMesh;
+                        }
+                }
+
+                public override Vector3 LocalScaling
+                {
+                        get
+                        {
+                                return _meshInterface.Scaling;
+                        }
+                        set
+                        {
+                                _meshInterface.Scaling = value;
+                        }
+                }
+
+                public override string Name
+                {
+                        get
+                        {
+                                return "TriangleMesh";
+                        }
+                }
+
+                public void RecalcLocalAabb()
+                {
+                        {
+                                Vector3 vec = new Vector3();
+                                vec.X = 1f;
+                                Vector3 tmp = LocalGetSupportingVertex(vec);
+                                _localAabbMax.X = tmp.X + CollisionMargin;
+                                vec.X = -1f;
+                                tmp = LocalGetSupportingVertex(vec);
+                                _localAabbMin.X = tmp.X - CollisionMargin;
+                        }
+                        {
+                                Vector3 vec = new Vector3();
+                                vec.Y = 1f;
+                                Vector3 tmp = LocalGetSupportingVertex(vec);
+                                _localAabbMax.Y = tmp.Y + CollisionMargin;
+                                vec.Y = -1f;
+                                tmp = LocalGetSupportingVertex(vec);
+                                _localAabbMin.Y = tmp.Y - CollisionMargin;
+                        }
+                        {
+                                Vector3 vec = new Vector3();
+                                vec.Z = 1f;
+                                Vector3 tmp = LocalGetSupportingVertex(vec);
+                                _localAabbMax.Z = tmp.Z + CollisionMargin;
+                                vec.Z = -1f;
+                                tmp = LocalGetSupportingVertex(vec);
+                                _localAabbMin.Z = tmp.Z - CollisionMargin;
+                        }
+                }
+
+                public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
+                {
+            LocalProcessAllTriangles(callback, aabbMax, aabbMax);
+                }
+
+        protected void LocalProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
+        {
+            FilteredCallback filterCallback = new FilteredCallback(callback, aabbMin, aabbMax);
+            _meshInterface.InternalProcessAllTriangles(filterCallback, aabbMin, aabbMax);
+        }
+
+        public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
+                {
+                        Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin);
+                        Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin);
+
+                        Matrix abs_b = MathHelper.Absolute(t);
+
+                        Vector3 center = MathHelper.MatrixToVector(t, localCenter);
+
+                        Vector3 extent = new Vector3(Vector3.Dot(new Vector3(abs_b.M11, abs_b.M12, abs_b.M13), localHalfExtents),
+                                   Vector3.Dot(new Vector3(abs_b.M21, abs_b.M22, abs_b.M23), localHalfExtents),
+                                  Vector3.Dot(new Vector3(abs_b.M31, abs_b.M32, abs_b.M33), localHalfExtents));
+                        extent += new Vector3(Margin, Margin, Margin);
+
+                        aabbMin = center - extent;
+                        aabbMax = center + extent;
+                }
+
+                public override void CalculateLocalInertia(float mass, out Vector3 inertia)
+                {
+                        inertia = new Vector3();
+                        //moving concave objects not supported
+                        BulletDebug.Assert(false);
+                }
+
+                public virtual Vector3 LocalGetSupportingVertex(Vector3 vec)
+                {
+                        Vector3 supportVertex;
+                        Matrix ident = Matrix.Identity;
+                        SupportVertexCallback supportCallback = new SupportVertexCallback(vec, ident);
+                        Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
+                        LocalProcessAllTriangles(supportCallback, -aabbMax, aabbMax);
+                        supportVertex = supportCallback.SupportVertexLocal;
+                        return supportVertex;
+                }
+
+                public virtual Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
+                {
+                        BulletDebug.Assert(false);
+                        return LocalGetSupportingVertex(vec);
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs
index 59ffad0..d5a17f3 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs
@@ -1,187 +1,187 @@
-/*

-  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 TriangleShape : PolyhedralConvexShape

-        {

-                private Vector3[] _vertices = new Vector3[3];

-

-                public TriangleShape(Vector3 pointA, Vector3 pointB, Vector3 pointC)

-                {

-                        _vertices[0] = pointA;

-                        _vertices[1] = pointB;

-                        _vertices[2] = pointC;

-                }

-

-                public override int PreferredPenetrationDirectionsCount

-                {

-                        get

-                        {

-                                return 2;

-                        }

-                }

-

-                public Vector3[] Vertices

-                {

-                        get

-                        {

-                                return _vertices;

-                        }

-                }

-

-                public override int VertexCount

-                {

-                        get

-                        {

-                                return 3;

-                        }

-                }

-

-                public override int EdgeCount

-                {

-                        get

-                        {

-                                return 3;

-                        }

-                }

-

-                public override int PlaneCount

-                {

-                        get

-                        {

-                                return 1;

-                        }

-                }

-

-                public override BroadphaseNativeTypes ShapeType

-                {

-                        get

-                        {

-                                return BroadphaseNativeTypes.Triangle;

-                        }

-                }

-

-                public override string Name

-                {

-                        get

-                        {

-                                return "Triangle";

-                        }

-                }

-

-                public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector)

-                {

-                        CalculateNormal(out penetrationVector);

-                        if (index != 0)

-                                penetrationVector *= -1f;

-                }

-

-                public virtual void GetPlaneEquation(int i, out Vector3 planeNormal, out Vector3 planeSupport)

-                {

-                        CalculateNormal(out planeNormal);

-                        planeSupport = _vertices[0];

-                }

-

-                public void CalculateNormal(out Vector3 normal)

-                {

-                        normal = Vector3.Normalize(Vector3.Cross(_vertices[1] - _vertices[0], _vertices[2] - _vertices[0]));

-                }

-

-                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)

-                {

-                        Vector3 dots = new Vector3(Vector3.Dot(vec, _vertices[0]), Vector3.Dot(vec, _vertices[1]), Vector3.Dot(vec, _vertices[2]));

-                        return _vertices[MathHelper.MaxAxis(dots)];

-                }

-

-                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)

-                {

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

-                        {

-                                Vector3 dir = vectors[i];

-                                Vector3 dots = new Vector3(Vector3.Dot(dir, _vertices[0]), Vector3.Dot(dir, _vertices[1]), Vector3.Dot(dir, _vertices[2]));

-                                supportVerticesOut[i] = _vertices[MathHelper.MaxAxis(dots)];

-                        }

-                }

-

-                public override void CalculateLocalInertia(float mass, out Vector3 inertia)

-                {

-                        inertia = new Vector3();

-                        BulletDebug.Assert(false);

-                }

-

-                public override void GetEdge(int i, out Vector3 pa, out Vector3 pb)

-                {

-                        GetVertex(i, out pa);

-                        GetVertex((i + 1) % 3, out pb);

-                }

-

-                public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)

-                {

-                        GetAabbSlow(t, out aabbMin, out aabbMax);

-                }

-

-                public override void GetVertex(int i, out Vector3 vtx)

-                {

-                        vtx = _vertices[i];

-                }

-

-                public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i)

-                {

-                        GetPlaneEquation(i, out planeNormal, out planeSupport);

-                }

-

-                public override bool IsInside(Vector3 pt, float tolerance)

-                {

-                        Vector3 normal;

-                        CalculateNormal(out normal);

-                        //distance to plane

-                        float dist = Vector3.Dot(pt, normal);

-                        float planeconst = Vector3.Dot(_vertices[0], normal);

-                        dist -= planeconst;

-                        if (dist >= -tolerance && dist <= tolerance)

-                        {

-                                //inside check on edge-planes

-                                int i;

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

-                                {

-                                        Vector3 pa, pb;

-                                        GetEdge(i, out pa, out pb);

-                                        Vector3 edge = pb - pa;

-                                        Vector3 edgeNormal = Vector3.Cross(edge, normal);

-                                        edgeNormal = Vector3.Normalize(edgeNormal);

-                                        float distance = Vector3.Dot(pt, edgeNormal);

-                                        float edgeConst = Vector3.Dot(pa, edgeNormal);

-                                        distance -= edgeConst;

-                                        if (distance < -tolerance)

-                                                return false;

-                                }

-                                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
+{
+        public class TriangleShape : PolyhedralConvexShape
+        {
+                private Vector3[] _vertices = new Vector3[3];
+
+                public TriangleShape(Vector3 pointA, Vector3 pointB, Vector3 pointC)
+                {
+                        _vertices[0] = pointA;
+                        _vertices[1] = pointB;
+                        _vertices[2] = pointC;
+                }
+
+                public override int PreferredPenetrationDirectionsCount
+                {
+                        get
+                        {
+                                return 2;
+                        }
+                }
+
+                public Vector3[] Vertices
+                {
+                        get
+                        {
+                                return _vertices;
+                        }
+                }
+
+                public override int VertexCount
+                {
+                        get
+                        {
+                                return 3;
+                        }
+                }
+
+                public override int EdgeCount
+                {
+                        get
+                        {
+                                return 3;
+                        }
+                }
+
+                public override int PlaneCount
+                {
+                        get
+                        {
+                                return 1;
+                        }
+                }
+
+                public override BroadphaseNativeTypes ShapeType
+                {
+                        get
+                        {
+                                return BroadphaseNativeTypes.Triangle;
+                        }
+                }
+
+                public override string Name
+                {
+                        get
+                        {
+                                return "Triangle";
+                        }
+                }
+
+                public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector)
+                {
+                        CalculateNormal(out penetrationVector);
+                        if (index != 0)
+                                penetrationVector *= -1f;
+                }
+
+                public virtual void GetPlaneEquation(int i, out Vector3 planeNormal, out Vector3 planeSupport)
+                {
+                        CalculateNormal(out planeNormal);
+                        planeSupport = _vertices[0];
+                }
+
+                public void CalculateNormal(out Vector3 normal)
+                {
+                        normal = Vector3.Normalize(Vector3.Cross(_vertices[1] - _vertices[0], _vertices[2] - _vertices[0]));
+                }
+
+                public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
+                {
+                        Vector3 dots = new Vector3(Vector3.Dot(vec, _vertices[0]), Vector3.Dot(vec, _vertices[1]), Vector3.Dot(vec, _vertices[2]));
+                        return _vertices[MathHelper.MaxAxis(dots)];
+                }
+
+                public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
+                {
+                        for (int i = 0; i < vectors.Length; i++)
+                        {
+                                Vector3 dir = vectors[i];
+                                Vector3 dots = new Vector3(Vector3.Dot(dir, _vertices[0]), Vector3.Dot(dir, _vertices[1]), Vector3.Dot(dir, _vertices[2]));
+                                supportVerticesOut[i] = _vertices[MathHelper.MaxAxis(dots)];
+                        }
+                }
+
+                public override void CalculateLocalInertia(float mass, out Vector3 inertia)
+                {
+                        inertia = new Vector3();
+                        BulletDebug.Assert(false);
+                }
+
+                public override void GetEdge(int i, out Vector3 pa, out Vector3 pb)
+                {
+                        GetVertex(i, out pa);
+                        GetVertex((i + 1) % 3, out pb);
+                }
+
+                public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
+                {
+                        GetAabbSlow(t, out aabbMin, out aabbMax);
+                }
+
+                public override void GetVertex(int i, out Vector3 vtx)
+                {
+                        vtx = _vertices[i];
+                }
+
+                public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i)
+                {
+                        GetPlaneEquation(i, out planeNormal, out planeSupport);
+                }
+
+                public override bool IsInside(Vector3 pt, float tolerance)
+                {
+                        Vector3 normal;
+                        CalculateNormal(out normal);
+                        //distance to plane
+                        float dist = Vector3.Dot(pt, normal);
+                        float planeconst = Vector3.Dot(_vertices[0], normal);
+                        dist -= planeconst;
+                        if (dist >= -tolerance && dist <= tolerance)
+                        {
+                                //inside check on edge-planes
+                                int i;
+                                for (i = 0; i < 3; i++)
+                                {
+                                        Vector3 pa, pb;
+                                        GetEdge(i, out pa, out pb);
+                                        Vector3 edge = pb - pa;
+                                        Vector3 edgeNormal = Vector3.Cross(edge, normal);
+                                        edgeNormal = Vector3.Normalize(edgeNormal);
+                                        float distance = Vector3.Dot(pt, edgeNormal);
+                                        float edgeConst = Vector3.Dot(pa, edgeNormal);
+                                        distance -= edgeConst;
+                                        if (distance < -tolerance)
+                                                return false;
+                                }
+                                return true;
+                        }
+                        return false;
+                }
+        }
+}
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;
+                }
+    }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs
index e4a9ae2..e731643 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs
@@ -1,488 +1,488 @@
-/*

-  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.Dynamics

-{

-        public delegate float ContactSolverFunc (RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo info);

-

-        public enum ContactSolverType

-        {

-                Default = 0,

-                TypeA,

-                TypeB,

-                User,

-                MaxContactSolverType,

-        }

-

-        public class ConstraintPersistentData

-        {

-                // total applied impulse during most recent frame

-                private float _appliedImpulse;

-                private float _previousAppliedImpulse;

-                private float _accumulatedTangentImpulse0;

-                private float _accumulatedTangentImpulse1;

-

-                private float _jacDiagABInv;

-                private float _jacDiagABInvTangentA;

-                private float _jacDiagABInvTangentB;

-                private int _persistentLifeTime;

-                private float _restitution;

-                private float _friction;

-                private float _penetration;

-                private Vector3 _frictionWorldTangentialA;

-                private Vector3 _frictionWorldTangentialB;

-

-                private Vector3 _frictionAngularComponent0A;

-                private Vector3 _frictionAngularComponent0B;

-                private Vector3 _frictionAngularComponent1A;

-                private Vector3 _frictionAngularComponent1B;

-

-                //some data doesn't need to be persistent over frames: todo: clean/reuse this

-                private Vector3 _angularComponentA;

-                private Vector3 _angularComponentB;

-

-                private ContactSolverFunc _contactSolverFunc;

-                private ContactSolverFunc _frictionSolverFunc;

-

-                public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } }

-                public float PreviousAppliedImpulse { get { return _previousAppliedImpulse; } set { _previousAppliedImpulse = value; } }

-                public float AccumulatedTangentImpulseA { get { return _accumulatedTangentImpulse0; } set { _accumulatedTangentImpulse0 = value; } }

-                public float AccumulatedTangentImpulseB { get { return _accumulatedTangentImpulse1; } set { _accumulatedTangentImpulse1 = value; } }

-                

-                public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } }

-                public float JacDiagABInvTangentA { get { return _jacDiagABInvTangentA; } set { _jacDiagABInvTangentA = value; } }

-                public float JacDiagABInvTangentB { get { return _jacDiagABInvTangentB; } set { _jacDiagABInvTangentB = value; } }

-                public int PersistentLifeTime { get { return _persistentLifeTime; } set { _persistentLifeTime = value; } }

-                public float Restitution { get { return _restitution; } set { _restitution = value; } }

-                public float Friction { get { return _friction; } set { _friction = value; } }

-                public float Penetration { get { return _penetration; } set { _penetration = value; } }

-                public Vector3 FrictionWorldTangentialA { get { return _frictionWorldTangentialA; } set { _frictionWorldTangentialA = value; } }

-                public Vector3 FrictionWorldTangentialB { get { return _frictionWorldTangentialB; } set { _frictionWorldTangentialB = value; } }

-

-                public Vector3 FrictionAngularComponent0A { get { return _frictionAngularComponent0A; } set { _frictionAngularComponent0A = value; } }

-                public Vector3 FrictionAngularComponent0B { get { return _frictionAngularComponent0B; } set { _frictionAngularComponent0B = value; } }

-                public Vector3 FrictionAngularComponent1A { get { return _frictionAngularComponent1A; } set { _frictionAngularComponent1A = value; } }

-                public Vector3 FrictionAngularComponent1B { get { return _frictionAngularComponent1B; } set { _frictionAngularComponent1B = value; } }

-

-                public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } }

-                public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } }

-

-                public ContactSolverFunc ContactSolverFunc { get { return _contactSolverFunc; } set { _contactSolverFunc = value; } }

-                public ContactSolverFunc FrictionSolverFunc { get { return _frictionSolverFunc; } set { _frictionSolverFunc = value; } }

-        }

-

-        public static class ContactConstraint

-        {

-                private const int UseInternalApplyImpulse = 1;

-

-                /// <summary>

-                /// bilateral constraint between two dynamic objects

-                /// positive distance = separation, negative distance = penetration

-                /// </summary>

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

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

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

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

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

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

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

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

-                public static void ResolveSingleBilateral(RigidBody bodyA, Vector3 posA,

-                                                          RigidBody bodyB, Vector3 posB,

-                                                          float distance, Vector3 normal, out float impulse, float timeStep)

-                {

-                        float normalLenSqr = normal.LengthSquared();

-

-                        if (Math.Abs(normalLenSqr) >= 1.1f)

-                                throw new BulletException();

-

-                        /*if (normalLenSqr > 1.1f)

-                        {

-                                impulse = 0f;

-                                return;

-                        }*/

-                        Vector3 rel_pos1 = posA - bodyA.CenterOfMassPosition;

-                        Vector3 rel_pos2 = posB - bodyB.CenterOfMassPosition;

-                        //this jacobian entry could be re-used for all iterations

-

-                        Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);

-                        Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);

-                        Vector3 vel = vel1 - vel2;

-

-

-                        JacobianEntry jac = new JacobianEntry(Matrix.Transpose(bodyA.CenterOfMassTransform),

-                                Matrix.Transpose(bodyB.CenterOfMassTransform),

-                                rel_pos1, rel_pos2, normal, bodyA.InvInertiaDiagLocal, bodyA.InverseMass,

-                                bodyB.InvInertiaDiagLocal, bodyB.InverseMass);

-

-                        float jacDiagAB = jac.Diagonal;

-                        float jacDiagABInv = 1f / jacDiagAB;

-

-                        float rel_vel = jac.GetRelativeVelocity(

-                          bodyA.LinearVelocity,

-                          Vector3.TransformNormal(bodyA.AngularVelocity, Matrix.Transpose(bodyA.CenterOfMassTransform)),

-                          bodyB.LinearVelocity,

-                          Vector3.TransformNormal(bodyB.AngularVelocity, Matrix.Transpose(bodyB.CenterOfMassTransform)));

-                        float a;

-                        a = jacDiagABInv;

-

-

-                        rel_vel = Vector3.Dot(normal, vel);

-

-                        float contactDamping = 0.2f;

-

-                        float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;

-                        impulse = velocityImpulse;

-                }

-

-

-                /// <summary>

-                /// contact constraint resolution:

-                /// calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint

-                /// positive distance = separation, negative distance = penetration

-                /// </summary>

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

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

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

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

-                /// <returns></returns>

-                public static float ResolveSingleCollision(RigidBody bodyA, RigidBody bodyB,

-                                ManifoldPoint contactPoint, ContactSolverInfo solverInfo)

-                {

-                        Vector3 pos1 = contactPoint.PositionWorldOnA;

-                        Vector3 pos2 = contactPoint.PositionWorldOnB;

-

-

-                        //      printf("distance=%f\n",distance);

-

-                        Vector3 normal = contactPoint.NormalWorldOnB;

-

-                        Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition;

-                        Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition;

-

-                        Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);

-                        Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);

-                        Vector3 vel = vel1 - vel2;

-                        float rel_vel;

-                        rel_vel = Vector3.Dot(normal, vel);

-

-

-                        float Kfps = 1f / solverInfo.TimeStep;

-

-                        //float damping = solverInfo.m_damping;

-                        float Kerp = solverInfo.Erp;

-

-                        float Kcor = Kerp * Kfps;

-

-                        //printf("dist=%f\n",distance);

-

-                        ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;

-                        if (cpd == null)

-                                throw new BulletException();

-

-                        float distance = cpd.Penetration;//contactPoint.getDistance();

-

-

-                        //distance = 0.f;

-                        float positionalError = Kcor * -distance;

-                        //jacDiagABInv;

-                        float velocityError = cpd.Restitution - rel_vel;// * damping;

-

-

-                        float penetrationImpulse = positionalError * cpd.JacDiagABInv;

-                        float velocityImpulse = velocityError * cpd.JacDiagABInv;

-                        float normalImpulse = penetrationImpulse + velocityImpulse;

-

-                        // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse

-                        float oldNormalImpulse = cpd.AppliedImpulse;

-                        float sum = oldNormalImpulse + normalImpulse;

-                        cpd.AppliedImpulse = 0f > sum ? 0f : sum;

-

-                        normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;

-

-                        if (bodyA.InverseMass != 0)

-                        {

-                                bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);

-                        }

-                        if (bodyB.InverseMass != 0)

-                        {

-                                bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);

-                        }

-

-                        /*body1.applyImpulse(normal * (normalImpulse), rel_pos1);

-                        body2.applyImpulse(-normal * (normalImpulse), rel_pos2);*/

-

-                        return normalImpulse;

-                }

-

-                public static float ResolveSingleFriction(RigidBody bodyA, RigidBody bodyB,

-                        ManifoldPoint contactPoint, ContactSolverInfo solverInfo)

-                {

-

-                        Vector3 pos1 = contactPoint.PositionWorldOnA;

-                        Vector3 pos2 = contactPoint.PositionWorldOnB;

-

-                        Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition;

-                        Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition;

-

-                        ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;

-                        if (cpd == null)

-                                throw new BulletException();

-

-                        float combinedFriction = cpd.Friction;

-

-                        float limit = cpd.AppliedImpulse * combinedFriction;

-

-                        //friction

-                        if (cpd.AppliedImpulse > 0)

-                        {

-                                //apply friction in the 2 tangential directions

-

-                                // 1st tangent

-                                Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);

-                                Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);

-                                Vector3 vel = vel1 - vel2;

-

-                                float j1, j2;

-

-                                {

-

-                                        float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);

-

-                                        // calculate j that moves us to zero relative velocity

-                                        j1 = -vrel * cpd.JacDiagABInvTangentA;

-                                        float oldTangentImpulse = cpd.AccumulatedTangentImpulseA;

-                                        cpd.AccumulatedTangentImpulseA = oldTangentImpulse + j1;

-                                        float atia = cpd.AccumulatedTangentImpulseA;

-                                        MathHelper.SetMin(ref atia, limit);

-                                        MathHelper.SetMax(ref atia, -limit);

-                                        cpd.AccumulatedTangentImpulseA = atia;

-                                        j1 = cpd.AccumulatedTangentImpulseA - oldTangentImpulse;

-

-                                }

-                                {

-                                        // 2nd tangent

-

-                                        float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);

-

-                                        // calculate j that moves us to zero relative velocity

-                                        j2 = -vrel * cpd.JacDiagABInvTangentB;

-                                        float oldTangentImpulse = cpd.AccumulatedTangentImpulseB;

-                                        cpd.AccumulatedTangentImpulseB = oldTangentImpulse + j2;

-                                        float atib = cpd.AccumulatedTangentImpulseB;

-                                        MathHelper.SetMin(ref atib, limit);

-                                        MathHelper.SetMax(ref atib, -limit);

-                                        cpd.AccumulatedTangentImpulseB = atib;

-                                        j2 = cpd.AccumulatedTangentImpulseB - oldTangentImpulse;

-                                }

-

-                                if (bodyA.InverseMass != 0)

-                                {

-                                        bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyA.InverseMass, cpd.FrictionAngularComponent0A, j1);

-                                        bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyA.InverseMass, cpd.FrictionAngularComponent1A, j2);

-                                }

-                                if (bodyB.InverseMass != 0)

-                                {

-                                        bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyB.InverseMass, cpd.FrictionAngularComponent0B, -j1);

-                                        bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyB.InverseMass, cpd.FrictionAngularComponent1B, -j2);

-                                }

-

-                        }

-                        return cpd.AppliedImpulse;

-                }

-

-                public static float ResolveSingleFrictionOriginal(

-                        RigidBody bodyA,

-                        RigidBody bodyB,

-                        ManifoldPoint contactPoint,

-                        ContactSolverInfo solverInfo)

-                {

-                        Vector3 posA = contactPoint.PositionWorldOnA;

-                        Vector3 posB = contactPoint.PositionWorldOnB;

-

-                        Vector3 relPosA = posA - bodyA.CenterOfMassPosition;

-                        Vector3 relPosB = posB - bodyB.CenterOfMassPosition;

-

-                        ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;

-                        if (cpd == null)

-                                throw new BulletException();

-

-                        float combinedFriction = cpd.Friction;

-

-                        float limit = cpd.AppliedImpulse * combinedFriction;

-                        //if (contactPoint.m_appliedImpulse>0.f)

-                        //friction

-                        {

-                                //apply friction in the 2 tangential directions

-

-                                {

-                                        // 1st tangent

-                                        Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);

-                                        Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);

-                                        Vector3 vel = velA - velB;

-

-                                        float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);

-

-                                        // calculate j that moves us to zero relative velocity

-                                        float j = -vrel * cpd.JacDiagABInvTangentA;

-                                        float total = cpd.AccumulatedTangentImpulseA + j;

-                                        if (limit < total)

-                                                total = limit;

-                                        if (total < -limit)

-                                                total = -limit;

-                                        j = total - cpd.AccumulatedTangentImpulseA;

-                                        cpd.AccumulatedTangentImpulseA = total;

-                                        bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA);

-                                        bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB);

-                                }

-

-

-                                {

-                                        // 2nd tangent

-                                        Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);

-                                        Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);

-                                        Vector3 vel = velA - velB;

-

-                                        float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);

-

-                                        // calculate j that moves us to zero relative velocity

-                                        float j = -vrel * cpd.JacDiagABInvTangentB;

-                                        float total = cpd.AccumulatedTangentImpulseB + j;

-                                        if (limit < total)

-                                                total = limit;

-                                        if (total < -limit)

-                                                total = -limit;

-                                        j = total - cpd.AccumulatedTangentImpulseB;

-                                        cpd.AccumulatedTangentImpulseB = total;

-                                        bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA);

-                                        bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB);

-                                }

-                        }

-

-                        return cpd.AppliedImpulse;

-                }

-

-                //velocity + friction

-                //response  between two dynamic objects with friction

-                public static float ResolveSingleCollisionCombined(

-                        RigidBody bodyA,

-                        RigidBody bodyB,

-                        ManifoldPoint contactPoint,

-                        ContactSolverInfo solverInfo)

-                {

-

-                        Vector3 posA = contactPoint.PositionWorldOnA;

-                        Vector3 posB = contactPoint.PositionWorldOnB;

-                        Vector3 normal = contactPoint.NormalWorldOnB;

-

-                        Vector3 relPosA = posA - bodyA.CenterOfMassPosition;

-                        Vector3 relPosB = posB - bodyB.CenterOfMassPosition;

-

-                        Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);

-                        Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);

-                        Vector3 vel = velA - velB;

-                        float relVel;

-                        relVel = Vector3.Dot(normal, vel);

-

-                        float Kfps = 1f / solverInfo.TimeStep;

-

-                        //float damping = solverInfo.m_damping;

-                        float Kerp = solverInfo.Erp;

-                        float Kcor = Kerp * Kfps;

-

-                        ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;

-                        if (cpd == null)

-                                throw new BulletException();

-

-                        float distance = cpd.Penetration;

-                        float positionalError = Kcor * -distance;

-                        float velocityError = cpd.Restitution - relVel;// * damping;

-

-                        float penetrationImpulse = positionalError * cpd.JacDiagABInv;

-

-                        float velocityImpulse = velocityError * cpd.JacDiagABInv;

-

-                        float normalImpulse = penetrationImpulse + velocityImpulse;

-

-                        // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse

-                        float oldNormalImpulse = cpd.AppliedImpulse;

-                        float sum = oldNormalImpulse + normalImpulse;

-                        cpd.AppliedImpulse = 0 > sum ? 0 : sum;

-

-                        normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;

-

-                        if (bodyA.InverseMass != 0)

-                        {

-                                bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);

-                        }

-                        if (bodyB.InverseMass != 0)

-                        {

-                                bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);

-                        }

-

-                        {

-                                //friction

-                                Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA);

-                                Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB);

-                                Vector3 vel3 = vel12 - vel22;

-

-                                relVel = Vector3.Dot(normal, vel3);

-

-

-                                Vector3 latVel = vel3 - normal * relVel;

-                                float lat_rel_vel = latVel.Length();

-

-                                float combinedFriction = cpd.Friction;

-

-                                if (cpd.AppliedImpulse > 0)

-                                        if (lat_rel_vel > float.Epsilon)

-                                        {

-                                                latVel /= lat_rel_vel;

-                                                Vector3 temp1 = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld);

-                                                Vector3 temp2 = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld);

-                                                float friction_impulse = lat_rel_vel /

-                                                        (bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB)));

-                                                float normal_impulse = cpd.AppliedImpulse * combinedFriction;

-

-                                                MathHelper.SetMin(ref friction_impulse, normal_impulse);

-                                                MathHelper.SetMin(ref friction_impulse, -normal_impulse);

-                                                bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA);

-                                                bodyB.ApplyImpulse(latVel * friction_impulse, relPosB);

-                                        }

-                        }

-                        return normalImpulse;

-                }

-

-                public static float ResolveSingleFrictionEmpty(

-                        RigidBody bodyA,

-                        RigidBody bodyB,

-                        ManifoldPoint contactPoint,

-                        ContactSolverInfo solverInfo)

-                {

-                        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.Dynamics
+{
+        public delegate float ContactSolverFunc (RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo info);
+
+        public enum ContactSolverType
+        {
+                Default = 0,
+                TypeA,
+                TypeB,
+                User,
+                MaxContactSolverType,
+        }
+
+        public class ConstraintPersistentData
+        {
+                // total applied impulse during most recent frame
+                private float _appliedImpulse;
+                private float _previousAppliedImpulse;
+                private float _accumulatedTangentImpulse0;
+                private float _accumulatedTangentImpulse1;
+
+                private float _jacDiagABInv;
+                private float _jacDiagABInvTangentA;
+                private float _jacDiagABInvTangentB;
+                private int _persistentLifeTime;
+                private float _restitution;
+                private float _friction;
+                private float _penetration;
+                private Vector3 _frictionWorldTangentialA;
+                private Vector3 _frictionWorldTangentialB;
+
+                private Vector3 _frictionAngularComponent0A;
+                private Vector3 _frictionAngularComponent0B;
+                private Vector3 _frictionAngularComponent1A;
+                private Vector3 _frictionAngularComponent1B;
+
+                //some data doesn't need to be persistent over frames: todo: clean/reuse this
+                private Vector3 _angularComponentA;
+                private Vector3 _angularComponentB;
+
+                private ContactSolverFunc _contactSolverFunc;
+                private ContactSolverFunc _frictionSolverFunc;
+
+                public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } }
+                public float PreviousAppliedImpulse { get { return _previousAppliedImpulse; } set { _previousAppliedImpulse = value; } }
+                public float AccumulatedTangentImpulseA { get { return _accumulatedTangentImpulse0; } set { _accumulatedTangentImpulse0 = value; } }
+                public float AccumulatedTangentImpulseB { get { return _accumulatedTangentImpulse1; } set { _accumulatedTangentImpulse1 = value; } }
+                
+                public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } }
+                public float JacDiagABInvTangentA { get { return _jacDiagABInvTangentA; } set { _jacDiagABInvTangentA = value; } }
+                public float JacDiagABInvTangentB { get { return _jacDiagABInvTangentB; } set { _jacDiagABInvTangentB = value; } }
+                public int PersistentLifeTime { get { return _persistentLifeTime; } set { _persistentLifeTime = value; } }
+                public float Restitution { get { return _restitution; } set { _restitution = value; } }
+                public float Friction { get { return _friction; } set { _friction = value; } }
+                public float Penetration { get { return _penetration; } set { _penetration = value; } }
+                public Vector3 FrictionWorldTangentialA { get { return _frictionWorldTangentialA; } set { _frictionWorldTangentialA = value; } }
+                public Vector3 FrictionWorldTangentialB { get { return _frictionWorldTangentialB; } set { _frictionWorldTangentialB = value; } }
+
+                public Vector3 FrictionAngularComponent0A { get { return _frictionAngularComponent0A; } set { _frictionAngularComponent0A = value; } }
+                public Vector3 FrictionAngularComponent0B { get { return _frictionAngularComponent0B; } set { _frictionAngularComponent0B = value; } }
+                public Vector3 FrictionAngularComponent1A { get { return _frictionAngularComponent1A; } set { _frictionAngularComponent1A = value; } }
+                public Vector3 FrictionAngularComponent1B { get { return _frictionAngularComponent1B; } set { _frictionAngularComponent1B = value; } }
+
+                public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } }
+                public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } }
+
+                public ContactSolverFunc ContactSolverFunc { get { return _contactSolverFunc; } set { _contactSolverFunc = value; } }
+                public ContactSolverFunc FrictionSolverFunc { get { return _frictionSolverFunc; } set { _frictionSolverFunc = value; } }
+        }
+
+        public static class ContactConstraint
+        {
+                private const int UseInternalApplyImpulse = 1;
+
+                /// <summary>
+                /// bilateral constraint between two dynamic objects
+                /// positive distance = separation, negative distance = penetration
+                /// </summary>
+                /// <param name="body1"></param>
+                /// <param name="pos1"></param>
+                /// <param name="body2"></param>
+                /// <param name="pos2"></param>
+                /// <param name="distance"></param>
+                /// <param name="normal"></param>
+                /// <param name="impulse"></param>
+                /// <param name="timeStep"></param>
+                public static void ResolveSingleBilateral(RigidBody bodyA, Vector3 posA,
+                                                          RigidBody bodyB, Vector3 posB,
+                                                          float distance, Vector3 normal, out float impulse, float timeStep)
+                {
+                        float normalLenSqr = normal.LengthSquared();
+
+                        if (Math.Abs(normalLenSqr) >= 1.1f)
+                                throw new BulletException();
+
+                        /*if (normalLenSqr > 1.1f)
+                        {
+                                impulse = 0f;
+                                return;
+                        }*/
+                        Vector3 rel_pos1 = posA - bodyA.CenterOfMassPosition;
+                        Vector3 rel_pos2 = posB - bodyB.CenterOfMassPosition;
+                        //this jacobian entry could be re-used for all iterations
+
+                        Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
+                        Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
+                        Vector3 vel = vel1 - vel2;
+
+
+                        JacobianEntry jac = new JacobianEntry(Matrix.Transpose(bodyA.CenterOfMassTransform),
+                                Matrix.Transpose(bodyB.CenterOfMassTransform),
+                                rel_pos1, rel_pos2, normal, bodyA.InvInertiaDiagLocal, bodyA.InverseMass,
+                                bodyB.InvInertiaDiagLocal, bodyB.InverseMass);
+
+                        float jacDiagAB = jac.Diagonal;
+                        float jacDiagABInv = 1f / jacDiagAB;
+
+                        float rel_vel = jac.GetRelativeVelocity(
+                          bodyA.LinearVelocity,
+                          Vector3.TransformNormal(bodyA.AngularVelocity, Matrix.Transpose(bodyA.CenterOfMassTransform)),
+                          bodyB.LinearVelocity,
+                          Vector3.TransformNormal(bodyB.AngularVelocity, Matrix.Transpose(bodyB.CenterOfMassTransform)));
+                        float a;
+                        a = jacDiagABInv;
+
+
+                        rel_vel = Vector3.Dot(normal, vel);
+
+                        float contactDamping = 0.2f;
+
+                        float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
+                        impulse = velocityImpulse;
+                }
+
+
+                /// <summary>
+                /// contact constraint resolution:
+                /// calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
+                /// positive distance = separation, negative distance = penetration
+                /// </summary>
+                /// <param name="body1"></param>
+                /// <param name="body2"></param>
+                /// <param name="contactPoint"></param>
+                /// <param name="info"></param>
+                /// <returns></returns>
+                public static float ResolveSingleCollision(RigidBody bodyA, RigidBody bodyB,
+                                ManifoldPoint contactPoint, ContactSolverInfo solverInfo)
+                {
+                        Vector3 pos1 = contactPoint.PositionWorldOnA;
+                        Vector3 pos2 = contactPoint.PositionWorldOnB;
+
+
+                        //      printf("distance=%f\n",distance);
+
+                        Vector3 normal = contactPoint.NormalWorldOnB;
+
+                        Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition;
+                        Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition;
+
+                        Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
+                        Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
+                        Vector3 vel = vel1 - vel2;
+                        float rel_vel;
+                        rel_vel = Vector3.Dot(normal, vel);
+
+
+                        float Kfps = 1f / solverInfo.TimeStep;
+
+                        //float damping = solverInfo.m_damping;
+                        float Kerp = solverInfo.Erp;
+
+                        float Kcor = Kerp * Kfps;
+
+                        //printf("dist=%f\n",distance);
+
+                        ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
+                        if (cpd == null)
+                                throw new BulletException();
+
+                        float distance = cpd.Penetration;//contactPoint.getDistance();
+
+
+                        //distance = 0.f;
+                        float positionalError = Kcor * -distance;
+                        //jacDiagABInv;
+                        float velocityError = cpd.Restitution - rel_vel;// * damping;
+
+
+                        float penetrationImpulse = positionalError * cpd.JacDiagABInv;
+                        float velocityImpulse = velocityError * cpd.JacDiagABInv;
+                        float normalImpulse = penetrationImpulse + velocityImpulse;
+
+                        // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+                        float oldNormalImpulse = cpd.AppliedImpulse;
+                        float sum = oldNormalImpulse + normalImpulse;
+                        cpd.AppliedImpulse = 0f > sum ? 0f : sum;
+
+                        normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;
+
+                        if (bodyA.InverseMass != 0)
+                        {
+                                bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);
+                        }
+                        if (bodyB.InverseMass != 0)
+                        {
+                                bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);
+                        }
+
+                        /*body1.applyImpulse(normal * (normalImpulse), rel_pos1);
+                        body2.applyImpulse(-normal * (normalImpulse), rel_pos2);*/
+
+                        return normalImpulse;
+                }
+
+                public static float ResolveSingleFriction(RigidBody bodyA, RigidBody bodyB,
+                        ManifoldPoint contactPoint, ContactSolverInfo solverInfo)
+                {
+
+                        Vector3 pos1 = contactPoint.PositionWorldOnA;
+                        Vector3 pos2 = contactPoint.PositionWorldOnB;
+
+                        Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition;
+                        Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition;
+
+                        ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
+                        if (cpd == null)
+                                throw new BulletException();
+
+                        float combinedFriction = cpd.Friction;
+
+                        float limit = cpd.AppliedImpulse * combinedFriction;
+
+                        //friction
+                        if (cpd.AppliedImpulse > 0)
+                        {
+                                //apply friction in the 2 tangential directions
+
+                                // 1st tangent
+                                Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
+                                Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
+                                Vector3 vel = vel1 - vel2;
+
+                                float j1, j2;
+
+                                {
+
+                                        float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);
+
+                                        // calculate j that moves us to zero relative velocity
+                                        j1 = -vrel * cpd.JacDiagABInvTangentA;
+                                        float oldTangentImpulse = cpd.AccumulatedTangentImpulseA;
+                                        cpd.AccumulatedTangentImpulseA = oldTangentImpulse + j1;
+                                        float atia = cpd.AccumulatedTangentImpulseA;
+                                        MathHelper.SetMin(ref atia, limit);
+                                        MathHelper.SetMax(ref atia, -limit);
+                                        cpd.AccumulatedTangentImpulseA = atia;
+                                        j1 = cpd.AccumulatedTangentImpulseA - oldTangentImpulse;
+
+                                }
+                                {
+                                        // 2nd tangent
+
+                                        float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);
+
+                                        // calculate j that moves us to zero relative velocity
+                                        j2 = -vrel * cpd.JacDiagABInvTangentB;
+                                        float oldTangentImpulse = cpd.AccumulatedTangentImpulseB;
+                                        cpd.AccumulatedTangentImpulseB = oldTangentImpulse + j2;
+                                        float atib = cpd.AccumulatedTangentImpulseB;
+                                        MathHelper.SetMin(ref atib, limit);
+                                        MathHelper.SetMax(ref atib, -limit);
+                                        cpd.AccumulatedTangentImpulseB = atib;
+                                        j2 = cpd.AccumulatedTangentImpulseB - oldTangentImpulse;
+                                }
+
+                                if (bodyA.InverseMass != 0)
+                                {
+                                        bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyA.InverseMass, cpd.FrictionAngularComponent0A, j1);
+                                        bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyA.InverseMass, cpd.FrictionAngularComponent1A, j2);
+                                }
+                                if (bodyB.InverseMass != 0)
+                                {
+                                        bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyB.InverseMass, cpd.FrictionAngularComponent0B, -j1);
+                                        bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyB.InverseMass, cpd.FrictionAngularComponent1B, -j2);
+                                }
+
+                        }
+                        return cpd.AppliedImpulse;
+                }
+
+                public static float ResolveSingleFrictionOriginal(
+                        RigidBody bodyA,
+                        RigidBody bodyB,
+                        ManifoldPoint contactPoint,
+                        ContactSolverInfo solverInfo)
+                {
+                        Vector3 posA = contactPoint.PositionWorldOnA;
+                        Vector3 posB = contactPoint.PositionWorldOnB;
+
+                        Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
+                        Vector3 relPosB = posB - bodyB.CenterOfMassPosition;
+
+                        ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
+                        if (cpd == null)
+                                throw new BulletException();
+
+                        float combinedFriction = cpd.Friction;
+
+                        float limit = cpd.AppliedImpulse * combinedFriction;
+                        //if (contactPoint.m_appliedImpulse>0.f)
+                        //friction
+                        {
+                                //apply friction in the 2 tangential directions
+
+                                {
+                                        // 1st tangent
+                                        Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
+                                        Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
+                                        Vector3 vel = velA - velB;
+
+                                        float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);
+
+                                        // calculate j that moves us to zero relative velocity
+                                        float j = -vrel * cpd.JacDiagABInvTangentA;
+                                        float total = cpd.AccumulatedTangentImpulseA + j;
+                                        if (limit < total)
+                                                total = limit;
+                                        if (total < -limit)
+                                                total = -limit;
+                                        j = total - cpd.AccumulatedTangentImpulseA;
+                                        cpd.AccumulatedTangentImpulseA = total;
+                                        bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA);
+                                        bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB);
+                                }
+
+
+                                {
+                                        // 2nd tangent
+                                        Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
+                                        Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
+                                        Vector3 vel = velA - velB;
+
+                                        float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);
+
+                                        // calculate j that moves us to zero relative velocity
+                                        float j = -vrel * cpd.JacDiagABInvTangentB;
+                                        float total = cpd.AccumulatedTangentImpulseB + j;
+                                        if (limit < total)
+                                                total = limit;
+                                        if (total < -limit)
+                                                total = -limit;
+                                        j = total - cpd.AccumulatedTangentImpulseB;
+                                        cpd.AccumulatedTangentImpulseB = total;
+                                        bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA);
+                                        bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB);
+                                }
+                        }
+
+                        return cpd.AppliedImpulse;
+                }
+
+                //velocity + friction
+                //response  between two dynamic objects with friction
+                public static float ResolveSingleCollisionCombined(
+                        RigidBody bodyA,
+                        RigidBody bodyB,
+                        ManifoldPoint contactPoint,
+                        ContactSolverInfo solverInfo)
+                {
+
+                        Vector3 posA = contactPoint.PositionWorldOnA;
+                        Vector3 posB = contactPoint.PositionWorldOnB;
+                        Vector3 normal = contactPoint.NormalWorldOnB;
+
+                        Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
+                        Vector3 relPosB = posB - bodyB.CenterOfMassPosition;
+
+                        Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
+                        Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
+                        Vector3 vel = velA - velB;
+                        float relVel;
+                        relVel = Vector3.Dot(normal, vel);
+
+                        float Kfps = 1f / solverInfo.TimeStep;
+
+                        //float damping = solverInfo.m_damping;
+                        float Kerp = solverInfo.Erp;
+                        float Kcor = Kerp * Kfps;
+
+                        ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
+                        if (cpd == null)
+                                throw new BulletException();
+
+                        float distance = cpd.Penetration;
+                        float positionalError = Kcor * -distance;
+                        float velocityError = cpd.Restitution - relVel;// * damping;
+
+                        float penetrationImpulse = positionalError * cpd.JacDiagABInv;
+
+                        float velocityImpulse = velocityError * cpd.JacDiagABInv;
+
+                        float normalImpulse = penetrationImpulse + velocityImpulse;
+
+                        // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+                        float oldNormalImpulse = cpd.AppliedImpulse;
+                        float sum = oldNormalImpulse + normalImpulse;
+                        cpd.AppliedImpulse = 0 > sum ? 0 : sum;
+
+                        normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;
+
+                        if (bodyA.InverseMass != 0)
+                        {
+                                bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);
+                        }
+                        if (bodyB.InverseMass != 0)
+                        {
+                                bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);
+                        }
+
+                        {
+                                //friction
+                                Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA);
+                                Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB);
+                                Vector3 vel3 = vel12 - vel22;
+
+                                relVel = Vector3.Dot(normal, vel3);
+
+
+                                Vector3 latVel = vel3 - normal * relVel;
+                                float lat_rel_vel = latVel.Length();
+
+                                float combinedFriction = cpd.Friction;
+
+                                if (cpd.AppliedImpulse > 0)
+                                        if (lat_rel_vel > float.Epsilon)
+                                        {
+                                                latVel /= lat_rel_vel;
+                                                Vector3 temp1 = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld);
+                                                Vector3 temp2 = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld);
+                                                float friction_impulse = lat_rel_vel /
+                                                        (bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB)));
+                                                float normal_impulse = cpd.AppliedImpulse * combinedFriction;
+
+                                                MathHelper.SetMin(ref friction_impulse, normal_impulse);
+                                                MathHelper.SetMin(ref friction_impulse, -normal_impulse);
+                                                bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA);
+                                                bodyB.ApplyImpulse(latVel * friction_impulse, relPosB);
+                                        }
+                        }
+                        return normalImpulse;
+                }
+
+                public static float ResolveSingleFrictionEmpty(
+                        RigidBody bodyA,
+                        RigidBody bodyB,
+                        ManifoldPoint contactPoint,
+                        ContactSolverInfo solverInfo)
+                {
+                        return 0;
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs
index c1763df..d3424f6 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs
@@ -1,62 +1,62 @@
-/*

-  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;

-

-namespace XnaDevRu.BulletX.Dynamics

-{

-        public class ContactSolverInfo

-        {

-                private float _tau;

-                private float _damping;

-                private float _friction;

-                private float _timeStep;

-                private float _restitution;

-                private int _numIterations;

-                private float _maxErrorReduction;

-                private float _sor;

-                private float _erp;

-

-                public ContactSolverInfo()

-                {

-                        _tau = 0.6f;

-                        _damping = 1.0f;

-                        _friction = 0.3f;

-                        _restitution = 0f;

-                        _maxErrorReduction = 20f;

-                        _numIterations = 10;

-                        _erp = 0.4f;

-                        _sor = 1.3f;

-                }

-

-                public float Tau { get { return _tau; } set { _tau = value; } }

-                public float Damping { get { return _damping; } set { _damping = value; } }

-                public float Friction { get { return _friction; } set { _friction = value; } }

-                public float TimeStep { get { return _timeStep; } set { _timeStep = value; } }

-                public float Restitution { get { return _restitution; } set { _restitution = value; } }

-                public int IterationsCount { get { return _numIterations; } set { _numIterations = value; } }

-                public float MaxErrorReduction { get { return _maxErrorReduction; } set { _maxErrorReduction = value; } }

-                public float Sor { get { return _sor; } set { _sor = value; } }

-                public float Erp { get { return _erp; } set { _erp = 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;
+
+namespace XnaDevRu.BulletX.Dynamics
+{
+        public class ContactSolverInfo
+        {
+                private float _tau;
+                private float _damping;
+                private float _friction;
+                private float _timeStep;
+                private float _restitution;
+                private int _numIterations;
+                private float _maxErrorReduction;
+                private float _sor;
+                private float _erp;
+
+                public ContactSolverInfo()
+                {
+                        _tau = 0.6f;
+                        _damping = 1.0f;
+                        _friction = 0.3f;
+                        _restitution = 0f;
+                        _maxErrorReduction = 20f;
+                        _numIterations = 10;
+                        _erp = 0.4f;
+                        _sor = 1.3f;
+                }
+
+                public float Tau { get { return _tau; } set { _tau = value; } }
+                public float Damping { get { return _damping; } set { _damping = value; } }
+                public float Friction { get { return _friction; } set { _friction = value; } }
+                public float TimeStep { get { return _timeStep; } set { _timeStep = value; } }
+                public float Restitution { get { return _restitution; } set { _restitution = value; } }
+                public int IterationsCount { get { return _numIterations; } set { _numIterations = value; } }
+                public float MaxErrorReduction { get { return _maxErrorReduction; } set { _maxErrorReduction = value; } }
+                public float Sor { get { return _sor; } set { _sor = value; } }
+                public float Erp { get { return _erp; } set { _erp = value; } }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs
index 479c863..f1e0b1e 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs
@@ -1,440 +1,440 @@
-/*

-  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.Dynamics

-{

-        /// <summary>

-        /// Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space

-        /// Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'

-        /// Work in progress (is still a Hinge actually)

-        /// </summary>

-        public class Generic6DofConstraint : TypedConstraint

-        {

-                private static readonly float[] _sign = { 1.0f, -1.0f, 1.0f };

-                private static readonly int[] _axisA = { 1, 0, 0 };

-                private static readonly int[] _axisB = { 2, 2, 1 };

-

-                private JacobianEntry[] _jacLinear = new JacobianEntry[3];                      // 3 orthogonal linear constraints

-                private JacobianEntry[] _jacAng = new JacobianEntry[3];         // 3 orthogonal angular constraints

-

-                private Matrix _frameInA;                       // the constraint space w.r.t body A

-                private Matrix _frameInB;                       // the constraint space w.r.t body B

-

-                private float[] _lowerLimit = new float[6];     // the constraint lower limits

-                private float[] _upperLimit = new float[6];     // the constraint upper limits

-

-                private float[] _accumulatedImpulse = new float[6];

-

-                public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, Matrix frameInA, Matrix frameInB)

-                        : base(rbA, rbB)

-                {

-                        _frameInA = frameInA;

-                        _frameInB = frameInB;

-                        //free means upper < lower, 

-                        //locked means upper == lower

-                        //limited means upper > lower

-                        //so start all locked

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

-                        {

-                                _lowerLimit[i] = 0.0f;

-                                _upperLimit[i] = 0.0f;

-                                _accumulatedImpulse[i] = 0.0f;

-                        }

-                }

-

-                public Generic6DofConstraint() { }

-

-                public void UpdateRHS(float timeStep) { }

-

-                public float ComputeAngle(int axis)

-                {

-                        float angle = 0;

-

-                        switch (axis)

-                        {

-                                case 0:

-                                        {

-                                                Vector3 v1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 1), RigidBodyA.CenterOfMassTransform);

-                                                Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform);

-                                                Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform);

-

-                                                float s = Vector3.Dot(v1, w2);

-                                                float c = Vector3.Dot(v1, v2);

-

-                                                angle = (float)Math.Atan2(s, c);

-                                                break;

-                                        }

-                                case 1:

-                                        {

-                                                Vector3 w1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 2), RigidBodyA.CenterOfMassTransform);

-                                                Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform);

-                                                Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform);

-

-                                                float s = Vector3.Dot(w1, u2);

-                                                float c = Vector3.Dot(w1, w2);

-

-                                                angle = (float)Math.Atan2(s, c);

-                                                break;

-                                        }

-                                case 2:

-                                        {

-                                                Vector3 u1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 0), RigidBodyA.CenterOfMassTransform);

-                                                Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform);

-                                                Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform);

-

-                                                float s = Vector3.Dot(u1, v2);

-                                                float c = Vector3.Dot(u1, u2);

-

-                                                angle = (float)Math.Atan2(s, c);

-                                                break;

-                                        }

-                                default: BulletDebug.Assert(false); break;

-                        }

-

-                        return angle;

-                }

-

-                public void SetLinearLowerLimit(Vector3 linearLower)

-                {

-                        _lowerLimit[0] = linearLower.X;

-                        _lowerLimit[1] = linearLower.Y;

-                        _lowerLimit[2] = linearLower.Z;

-                }

-

-                public void SetLinearUpperLimit(Vector3 linearUpper)

-                {

-                        _upperLimit[0] = linearUpper.X;

-                        _upperLimit[1] = linearUpper.Y;

-                        _upperLimit[2] = linearUpper.Z;

-                }

-

-                public void SetAngularLowerLimit(Vector3 angularLower)

-                {

-                        _lowerLimit[3] = angularLower.X;

-                        _lowerLimit[4] = angularLower.Y;

-                        _lowerLimit[5] = angularLower.Z;

-                }

-

-                public void SetAngularUpperLimit(Vector3 angularUpper)

-                {

-                        _upperLimit[3] = angularUpper.X;

-                        _upperLimit[4] = angularUpper.Y;

-                        _upperLimit[5] = angularUpper.Z;

-                }

-

-                //first 3 are linear, next 3 are angular

-                public void SetLimit(int axis, float lo, float hi)

-                {

-                        _lowerLimit[axis] = lo;

-                        _upperLimit[axis] = hi;

-                }

-

-                //free means upper < lower, 

-                //locked means upper == lower

-                //limited means upper > lower

-                //limitIndex: first 3 are linear, next 3 are angular

-                public bool IsLimited(int limitIndex)

-                {

-                        return (_upperLimit[limitIndex] >= _lowerLimit[limitIndex]);

-                }

-

-                public override void BuildJacobian()

-                {

-                        Vector3 localNormalInA = new Vector3(0, 0, 0);

-

-                        Vector3 pivotInA = _frameInA.Translation;

-                        Vector3 pivotInB = _frameInB.Translation;

-

-                        Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform);

-                        Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform);

-

-                        Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;

-                        Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;

-

-                        //linear part

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

-                        {

-                                if (IsLimited(i))

-                                {

-                                        if (i == 0)

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

-                                        else if (i == 1)

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

-                                        else

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

-

-                                        Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform);

-

-                                        // Create linear atom

-                                        _jacLinear[i] = new JacobianEntry(

-                                                MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),

-                                                MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),

-                                                MathHelper.Transform(pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition,

-                                                MathHelper.Transform(pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition,

-                                                normalWorld,

-                                                RigidBodyA.InvInertiaDiagLocal,

-                                                RigidBodyA.InverseMass,

-                                                RigidBodyB.InvInertiaDiagLocal,

-                                                RigidBodyB.InverseMass);

-

-                                        //optionally disable warmstarting

-                                        _accumulatedImpulse[i] = 0f;

-

-                                        // Apply accumulated impulse

-                                        Vector3 impulse_vector = _accumulatedImpulse[i] * normalWorld;

-

-                                        RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1);

-                                        RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2);

-                                }

-                        }

-

-                        // angular part

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

-                        {

-                                if (IsLimited(i + 3))

-                                {

-                                        Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform);

-                                        Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform);

-

-                                        Vector3 axis = _sign[i] * Vector3.Cross(axisA, axisB);

-

-                                        // Create angular atom

-                                        _jacAng[i] = new JacobianEntry(axis,

-                                                MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),

-                                                MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),

-                                                RigidBodyA.InvInertiaDiagLocal,

-                                                RigidBodyB.InvInertiaDiagLocal);

-

-                                        _accumulatedImpulse[i + 3] = 0f;

-

-                                        // Apply accumulated impulse

-                                        Vector3 impulse_vector = _accumulatedImpulse[i + 3] * axis;

-

-                                        RigidBodyA.ApplyTorqueImpulse(impulse_vector);

-                                        RigidBodyB.ApplyTorqueImpulse(-impulse_vector);

-                                }

-                        }

-                }

-

-                public override void SolveConstraint(float timeStep)

-                {

-                        float tau = 0.1f;

-                        float damping = 1.0f;

-

-                        Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform);

-                        Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform);

-

-                        Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;

-                        Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;

-

-                        Vector3 localNormalInA = new Vector3();

-

-                        // linear

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

-                        {

-                                if (IsLimited(i))

-                                {

-                                        Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform));

-                                        Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform));

-

-                                        if (i == 0)

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

-                                        else if (i == 1)

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

-                                        else

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

-

-                                        Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform);

-

-                                        float jacDiagABInv = 1f / _jacLinear[i].Diagonal;

-

-                                        //velocity error (first order error)

-                                        float rel_vel = _jacLinear[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA,

-                                                                                                                                                        RigidBodyB.LinearVelocity, angvelB);

-

-                                        //positional error (zeroth order error)

-                                        float depth = -Vector3.Dot(pivotAInW - pivotBInW, normalWorld);

-                                        float lo = -1e30f;

-                                        float hi = 1e30f;

-

-                                        //handle the limits

-                                        if (_lowerLimit[i] < _upperLimit[i])

-                                        {

-                                                if (depth > _upperLimit[i])

-                                                {

-                                                        depth -= _upperLimit[i];

-                                                        lo = 0f;

-                                                }

-                                                else

-                                                {

-                                                        if (depth < _lowerLimit[i])

-                                                        {

-                                                                depth -= _lowerLimit[i];

-                                                                hi = 0f;

-                                                        }

-                                                        else

-                                                        {

-                                                                continue;

-                                                        }

-                                                }

-                                        }

-

-                                        float normalImpulse = (tau * depth / timeStep - damping * rel_vel) * jacDiagABInv;

-                                        float oldNormalImpulse = _accumulatedImpulse[i];

-                                        float sum = oldNormalImpulse + normalImpulse;

-                                        _accumulatedImpulse[i] = sum > hi ? 0f : sum < lo ? 0f : sum;

-                                        normalImpulse = _accumulatedImpulse[i] - oldNormalImpulse;

-

-                                        Vector3 impulse_vector = normalWorld * normalImpulse;

-                                        RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1);

-                                        RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2);

-                                }

-                        }

-

-                        Vector3 axis;

-                        float angle;

-                        Matrix frameAWorld = RigidBodyA.CenterOfMassTransform * _frameInA;

-                        Matrix frameBWorld = RigidBodyB.CenterOfMassTransform * _frameInB;

-

-                        TransformUtil.CalculateDiffAxisAngle(frameAWorld, frameBWorld, out axis, out angle);

-                        Quaternion diff = new Quaternion(axis, angle);

-                        Matrix diffMat = Matrix.CreateFromQuaternion(diff);

-                        Vector3 xyz;

-                        // this is not perfect, we can first check which axis are limited, and choose a more appropriate order

-                        MatrixToEulerXYZ(diffMat, out xyz);

-

-                        // angular

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

-                        {

-                                if (IsLimited(i + 3))

-                                {

-                                        Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform));

-                                        Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform));

-

-                                        float jacDiagABInv = 1f / _jacAng[i].Diagonal;

-

-                                        //velocity error (first order error)

-                                        float rel_vel = _jacAng[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA,

-                                                                                                                                                                        RigidBodyB.LinearVelocity, angvelB);

-

-                                        //positional error (zeroth order error)

-                                        Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform);

-                                        Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform);

-

-                                        float rel_pos = _sign[i] * Vector3.Dot(axisA, axisB);

-

-                                        float lo = -1e30f;

-                                        float hi = 1e30f;

-

-                                        //handle the twist limit

-                                        if (_lowerLimit[i + 3] < _upperLimit[i + 3])

-                                        {

-                                                //clamp the values

-                                                float loLimit = _upperLimit[i + 3] > -3.1415 ? _lowerLimit[i + 3] : -1e30f;

-                                                float hiLimit = _upperLimit[i + 3] < 3.1415 ? _upperLimit[i + 3] : 1e30f;

-

-                                                float projAngle;

-

-                                                if (i == 0)

-                                                        projAngle = -2f * xyz.Z;

-                                                else if (i == 1)

-                                                        projAngle = -2f * xyz.Y;

-                                                else

-                                                        projAngle = -2f * xyz.Z;

-

-                                                if (projAngle < loLimit)

-                                                {

-                                                        hi = 0f;

-                                                        rel_pos = loLimit - projAngle;

-                                                }

-                                                else

-                                                {

-                                                        if (projAngle > hiLimit)

-                                                        {

-                                                                lo = 0f;

-                                                                rel_pos = (hiLimit - projAngle);

-                                                        }

-                                                        else

-                                                        {

-                                                                continue;

-                                                        }

-                                                }

-                                        }

-

-                                        //impulse

-

-                                        float normalImpulse = -(tau * rel_pos / timeStep + damping * rel_vel) * jacDiagABInv;

-                                        float oldNormalImpulse = _accumulatedImpulse[i + 3];

-                                        float sum = oldNormalImpulse + normalImpulse;

-                                        _accumulatedImpulse[i + 3] = sum > hi ? 0f : sum < lo ? 0f : sum;

-                                        normalImpulse = _accumulatedImpulse[i + 3] - oldNormalImpulse;

-

-                                        Vector3 axis2 = _sign[i] * Vector3.Cross(axisA, axisB);

-                                        Vector3 impulse_vector = axis2 * normalImpulse;

-

-                                        RigidBodyA.ApplyTorqueImpulse(impulse_vector);

-                                        RigidBodyB.ApplyTorqueImpulse(-impulse_vector);

-                                }

-                        }

-                }

-

-                //MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html

-                private bool MatrixToEulerXYZ(Matrix mat, out Vector3 xyz)

-                {

-                        // rot =  cy*cz          -cy*sz           sy

-                        //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx

-                        //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy

-                        xyz = new Vector3();

-

-                        if (MathHelper.GetElement(mat, 2) < 1.0f)

-                        {

-                                if (MathHelper.GetElement(mat, 2) > -1.0f)

-                                {

-                                        xyz.X = (float)Math.Atan2(-MathHelper.GetElement(mat, 5), MathHelper.GetElement(mat, 8));

-                                        xyz.Y = (float)Math.Asin(MathHelper.GetElement(mat, 2));

-                                        xyz.Z = (float)Math.Atan2(-MathHelper.GetElement(mat, 1), MathHelper.GetElement(mat, 0));

-                                        return true;

-                                }

-                                else

-                                {

-                                        // WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)

-                                        xyz.X = -(float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4));

-                                        xyz.Y = -(float)Math.PI / 2;

-                                        xyz.Z = 0.0f;

-                                        return false;

-                                }

-                        }

-                        else

-                        {

-                                // WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)

-                                xyz.X = (float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4));

-                                xyz.Y = (float)Math.PI / 2;

-                                xyz.Z = 0.0f;

-                                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.Dynamics
+{
+        /// <summary>
+        /// Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
+        /// Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'
+        /// Work in progress (is still a Hinge actually)
+        /// </summary>
+        public class Generic6DofConstraint : TypedConstraint
+        {
+                private static readonly float[] _sign = { 1.0f, -1.0f, 1.0f };
+                private static readonly int[] _axisA = { 1, 0, 0 };
+                private static readonly int[] _axisB = { 2, 2, 1 };
+
+                private JacobianEntry[] _jacLinear = new JacobianEntry[3];                      // 3 orthogonal linear constraints
+                private JacobianEntry[] _jacAng = new JacobianEntry[3];         // 3 orthogonal angular constraints
+
+                private Matrix _frameInA;                       // the constraint space w.r.t body A
+                private Matrix _frameInB;                       // the constraint space w.r.t body B
+
+                private float[] _lowerLimit = new float[6];     // the constraint lower limits
+                private float[] _upperLimit = new float[6];     // the constraint upper limits
+
+                private float[] _accumulatedImpulse = new float[6];
+
+                public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, Matrix frameInA, Matrix frameInB)
+                        : base(rbA, rbB)
+                {
+                        _frameInA = frameInA;
+                        _frameInB = frameInB;
+                        //free means upper < lower, 
+                        //locked means upper == lower
+                        //limited means upper > lower
+                        //so start all locked
+                        for (int i = 0; i < 6; ++i)
+                        {
+                                _lowerLimit[i] = 0.0f;
+                                _upperLimit[i] = 0.0f;
+                                _accumulatedImpulse[i] = 0.0f;
+                        }
+                }
+
+                public Generic6DofConstraint() { }
+
+                public void UpdateRHS(float timeStep) { }
+
+                public float ComputeAngle(int axis)
+                {
+                        float angle = 0;
+
+                        switch (axis)
+                        {
+                                case 0:
+                                        {
+                                                Vector3 v1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 1), RigidBodyA.CenterOfMassTransform);
+                                                Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform);
+                                                Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform);
+
+                                                float s = Vector3.Dot(v1, w2);
+                                                float c = Vector3.Dot(v1, v2);
+
+                                                angle = (float)Math.Atan2(s, c);
+                                                break;
+                                        }
+                                case 1:
+                                        {
+                                                Vector3 w1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 2), RigidBodyA.CenterOfMassTransform);
+                                                Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform);
+                                                Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform);
+
+                                                float s = Vector3.Dot(w1, u2);
+                                                float c = Vector3.Dot(w1, w2);
+
+                                                angle = (float)Math.Atan2(s, c);
+                                                break;
+                                        }
+                                case 2:
+                                        {
+                                                Vector3 u1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 0), RigidBodyA.CenterOfMassTransform);
+                                                Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform);
+                                                Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform);
+
+                                                float s = Vector3.Dot(u1, v2);
+                                                float c = Vector3.Dot(u1, u2);
+
+                                                angle = (float)Math.Atan2(s, c);
+                                                break;
+                                        }
+                                default: BulletDebug.Assert(false); break;
+                        }
+
+                        return angle;
+                }
+
+                public void SetLinearLowerLimit(Vector3 linearLower)
+                {
+                        _lowerLimit[0] = linearLower.X;
+                        _lowerLimit[1] = linearLower.Y;
+                        _lowerLimit[2] = linearLower.Z;
+                }
+
+                public void SetLinearUpperLimit(Vector3 linearUpper)
+                {
+                        _upperLimit[0] = linearUpper.X;
+                        _upperLimit[1] = linearUpper.Y;
+                        _upperLimit[2] = linearUpper.Z;
+                }
+
+                public void SetAngularLowerLimit(Vector3 angularLower)
+                {
+                        _lowerLimit[3] = angularLower.X;
+                        _lowerLimit[4] = angularLower.Y;
+                        _lowerLimit[5] = angularLower.Z;
+                }
+
+                public void SetAngularUpperLimit(Vector3 angularUpper)
+                {
+                        _upperLimit[3] = angularUpper.X;
+                        _upperLimit[4] = angularUpper.Y;
+                        _upperLimit[5] = angularUpper.Z;
+                }
+
+                //first 3 are linear, next 3 are angular
+                public void SetLimit(int axis, float lo, float hi)
+                {
+                        _lowerLimit[axis] = lo;
+                        _upperLimit[axis] = hi;
+                }
+
+                //free means upper < lower, 
+                //locked means upper == lower
+                //limited means upper > lower
+                //limitIndex: first 3 are linear, next 3 are angular
+                public bool IsLimited(int limitIndex)
+                {
+                        return (_upperLimit[limitIndex] >= _lowerLimit[limitIndex]);
+                }
+
+                public override void BuildJacobian()
+                {
+                        Vector3 localNormalInA = new Vector3(0, 0, 0);
+
+                        Vector3 pivotInA = _frameInA.Translation;
+                        Vector3 pivotInB = _frameInB.Translation;
+
+                        Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform);
+                        Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform);
+
+                        Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
+                        Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;
+
+                        //linear part
+                        for (int i = 0; i < 3; i++)
+                        {
+                                if (IsLimited(i))
+                                {
+                                        if (i == 0)
+                                                localNormalInA = new Vector3(1, 0, 0);
+                                        else if (i == 1)
+                                                localNormalInA = new Vector3(0, 1, 0);
+                                        else
+                                                localNormalInA = new Vector3(0, 0, 1);
+
+                                        Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform);
+
+                                        // Create linear atom
+                                        _jacLinear[i] = new JacobianEntry(
+                                                MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
+                                                MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
+                                                MathHelper.Transform(pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition,
+                                                MathHelper.Transform(pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition,
+                                                normalWorld,
+                                                RigidBodyA.InvInertiaDiagLocal,
+                                                RigidBodyA.InverseMass,
+                                                RigidBodyB.InvInertiaDiagLocal,
+                                                RigidBodyB.InverseMass);
+
+                                        //optionally disable warmstarting
+                                        _accumulatedImpulse[i] = 0f;
+
+                                        // Apply accumulated impulse
+                                        Vector3 impulse_vector = _accumulatedImpulse[i] * normalWorld;
+
+                                        RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1);
+                                        RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2);
+                                }
+                        }
+
+                        // angular part
+                        for (int i = 0; i < 3; i++)
+                        {
+                                if (IsLimited(i + 3))
+                                {
+                                        Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform);
+                                        Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform);
+
+                                        Vector3 axis = _sign[i] * Vector3.Cross(axisA, axisB);
+
+                                        // Create angular atom
+                                        _jacAng[i] = new JacobianEntry(axis,
+                                                MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
+                                                MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
+                                                RigidBodyA.InvInertiaDiagLocal,
+                                                RigidBodyB.InvInertiaDiagLocal);
+
+                                        _accumulatedImpulse[i + 3] = 0f;
+
+                                        // Apply accumulated impulse
+                                        Vector3 impulse_vector = _accumulatedImpulse[i + 3] * axis;
+
+                                        RigidBodyA.ApplyTorqueImpulse(impulse_vector);
+                                        RigidBodyB.ApplyTorqueImpulse(-impulse_vector);
+                                }
+                        }
+                }
+
+                public override void SolveConstraint(float timeStep)
+                {
+                        float tau = 0.1f;
+                        float damping = 1.0f;
+
+                        Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform);
+                        Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform);
+
+                        Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
+                        Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;
+
+                        Vector3 localNormalInA = new Vector3();
+
+                        // linear
+                        for (int i = 0; i < 3; i++)
+                        {
+                                if (IsLimited(i))
+                                {
+                                        Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform));
+                                        Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform));
+
+                                        if (i == 0)
+                                                localNormalInA = new Vector3(1, 0, 0);
+                                        else if (i == 1)
+                                                localNormalInA = new Vector3(0, 1, 0);
+                                        else
+                                                localNormalInA = new Vector3(0, 0, 1);
+
+                                        Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform);
+
+                                        float jacDiagABInv = 1f / _jacLinear[i].Diagonal;
+
+                                        //velocity error (first order error)
+                                        float rel_vel = _jacLinear[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA,
+                                                                                                                                                        RigidBodyB.LinearVelocity, angvelB);
+
+                                        //positional error (zeroth order error)
+                                        float depth = -Vector3.Dot(pivotAInW - pivotBInW, normalWorld);
+                                        float lo = -1e30f;
+                                        float hi = 1e30f;
+
+                                        //handle the limits
+                                        if (_lowerLimit[i] < _upperLimit[i])
+                                        {
+                                                if (depth > _upperLimit[i])
+                                                {
+                                                        depth -= _upperLimit[i];
+                                                        lo = 0f;
+                                                }
+                                                else
+                                                {
+                                                        if (depth < _lowerLimit[i])
+                                                        {
+                                                                depth -= _lowerLimit[i];
+                                                                hi = 0f;
+                                                        }
+                                                        else
+                                                        {
+                                                                continue;
+                                                        }
+                                                }
+                                        }
+
+                                        float normalImpulse = (tau * depth / timeStep - damping * rel_vel) * jacDiagABInv;
+                                        float oldNormalImpulse = _accumulatedImpulse[i];
+                                        float sum = oldNormalImpulse + normalImpulse;
+                                        _accumulatedImpulse[i] = sum > hi ? 0f : sum < lo ? 0f : sum;
+                                        normalImpulse = _accumulatedImpulse[i] - oldNormalImpulse;
+
+                                        Vector3 impulse_vector = normalWorld * normalImpulse;
+                                        RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1);
+                                        RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2);
+                                }
+                        }
+
+                        Vector3 axis;
+                        float angle;
+                        Matrix frameAWorld = RigidBodyA.CenterOfMassTransform * _frameInA;
+                        Matrix frameBWorld = RigidBodyB.CenterOfMassTransform * _frameInB;
+
+                        TransformUtil.CalculateDiffAxisAngle(frameAWorld, frameBWorld, out axis, out angle);
+                        Quaternion diff = new Quaternion(axis, angle);
+                        Matrix diffMat = Matrix.CreateFromQuaternion(diff);
+                        Vector3 xyz;
+                        // this is not perfect, we can first check which axis are limited, and choose a more appropriate order
+                        MatrixToEulerXYZ(diffMat, out xyz);
+
+                        // angular
+                        for (int i = 0; i < 3; i++)
+                        {
+                                if (IsLimited(i + 3))
+                                {
+                                        Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform));
+                                        Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform));
+
+                                        float jacDiagABInv = 1f / _jacAng[i].Diagonal;
+
+                                        //velocity error (first order error)
+                                        float rel_vel = _jacAng[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA,
+                                                                                                                                                                        RigidBodyB.LinearVelocity, angvelB);
+
+                                        //positional error (zeroth order error)
+                                        Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform);
+                                        Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform);
+
+                                        float rel_pos = _sign[i] * Vector3.Dot(axisA, axisB);
+
+                                        float lo = -1e30f;
+                                        float hi = 1e30f;
+
+                                        //handle the twist limit
+                                        if (_lowerLimit[i + 3] < _upperLimit[i + 3])
+                                        {
+                                                //clamp the values
+                                                float loLimit = _upperLimit[i + 3] > -3.1415 ? _lowerLimit[i + 3] : -1e30f;
+                                                float hiLimit = _upperLimit[i + 3] < 3.1415 ? _upperLimit[i + 3] : 1e30f;
+
+                                                float projAngle;
+
+                                                if (i == 0)
+                                                        projAngle = -2f * xyz.Z;
+                                                else if (i == 1)
+                                                        projAngle = -2f * xyz.Y;
+                                                else
+                                                        projAngle = -2f * xyz.Z;
+
+                                                if (projAngle < loLimit)
+                                                {
+                                                        hi = 0f;
+                                                        rel_pos = loLimit - projAngle;
+                                                }
+                                                else
+                                                {
+                                                        if (projAngle > hiLimit)
+                                                        {
+                                                                lo = 0f;
+                                                                rel_pos = (hiLimit - projAngle);
+                                                        }
+                                                        else
+                                                        {
+                                                                continue;
+                                                        }
+                                                }
+                                        }
+
+                                        //impulse
+
+                                        float normalImpulse = -(tau * rel_pos / timeStep + damping * rel_vel) * jacDiagABInv;
+                                        float oldNormalImpulse = _accumulatedImpulse[i + 3];
+                                        float sum = oldNormalImpulse + normalImpulse;
+                                        _accumulatedImpulse[i + 3] = sum > hi ? 0f : sum < lo ? 0f : sum;
+                                        normalImpulse = _accumulatedImpulse[i + 3] - oldNormalImpulse;
+
+                                        Vector3 axis2 = _sign[i] * Vector3.Cross(axisA, axisB);
+                                        Vector3 impulse_vector = axis2 * normalImpulse;
+
+                                        RigidBodyA.ApplyTorqueImpulse(impulse_vector);
+                                        RigidBodyB.ApplyTorqueImpulse(-impulse_vector);
+                                }
+                        }
+                }
+
+                //MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
+                private bool MatrixToEulerXYZ(Matrix mat, out Vector3 xyz)
+                {
+                        // rot =  cy*cz          -cy*sz           sy
+                        //        cz*sx*sy+cx*sz  cx*cz-sx*sy*sz -cy*sx
+                        //       -cx*cz*sy+sx*sz  cz*sx+cx*sy*sz  cx*cy
+                        xyz = new Vector3();
+
+                        if (MathHelper.GetElement(mat, 2) < 1.0f)
+                        {
+                                if (MathHelper.GetElement(mat, 2) > -1.0f)
+                                {
+                                        xyz.X = (float)Math.Atan2(-MathHelper.GetElement(mat, 5), MathHelper.GetElement(mat, 8));
+                                        xyz.Y = (float)Math.Asin(MathHelper.GetElement(mat, 2));
+                                        xyz.Z = (float)Math.Atan2(-MathHelper.GetElement(mat, 1), MathHelper.GetElement(mat, 0));
+                                        return true;
+                                }
+                                else
+                                {
+                                        // WARNING.  Not unique.  XA - ZA = -atan2(r10,r11)
+                                        xyz.X = -(float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4));
+                                        xyz.Y = -(float)Math.PI / 2;
+                                        xyz.Z = 0.0f;
+                                        return false;
+                                }
+                        }
+                        else
+                        {
+                                // WARNING.  Not unique.  XAngle + ZAngle = atan2(r10,r11)
+                                xyz.X = (float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4));
+                                xyz.Y = (float)Math.PI / 2;
+                                xyz.Z = 0.0f;
+                                return false;
+                        }
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs
index 3e35550..02bcf8a 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.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.Dynamics

-{

-        /// <summary>

-        /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space

-        /// axis defines the orientation of the hinge axis

-        /// </summary>

-        public class HingeConstraint : TypedConstraint

-        {

-                private JacobianEntry[] _jac = new JacobianEntry[3]; //3 orthogonal linear constraints

-                private JacobianEntry[] _jacAng = new JacobianEntry[3]; //2 orthogonal angular constraints + 1 for limit/motor

-

-                private Vector3 _pivotInA;

-                private Vector3 _pivotInB;

-                private Vector3 _axisInA;

-                private Vector3 _axisInB;

-

-                private bool _angularOnly;

-

-                private float _motorTargetVelocity;

-                private float _maxMotorImpulse;

-                private bool _enableAngularMotor;

-

-                public HingeConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB, Vector3 axisInA, Vector3 axisInB)

-                        : base(rbA, rbB)

-                {

-                        _pivotInA = pivotInA;

-                        _pivotInB = pivotInB;

-                        _axisInA = axisInA;

-                        _axisInB = -axisInB;

-                        _angularOnly = false;

-                }

-

-                public HingeConstraint(RigidBody rbA, Vector3 pivotInA, Vector3 axisInA)

-                        : base(rbA)

-                {

-                        _pivotInA = pivotInA;

-                        _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, pivotInA);

-                        _axisInA = axisInA;

-                        //fixed axis in worldspace

-                        _axisInB = MathHelper.TransformNormal(-axisInA, rbA.CenterOfMassTransform);

-                        _angularOnly = false;

-                }

-

-                public HingeConstraint() { }

-

-                public bool AngularOnly { set { _angularOnly = value; } }

-

-                public override void BuildJacobian()

-                {

-                        AppliedImpulse = 0f;

-

-                        Vector3 normal = new Vector3();

-

-                        if (!_angularOnly)

-                        {

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

-                                {

-                                        MathHelper.SetElement(ref normal, i, 1);

-                                        _jac[i] = new JacobianEntry(

-                                                        MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),

-                                                        MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),

-                                                        MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition,

-                                                        MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition,

-                                                        normal,

-                                                        RigidBodyA.InvInertiaDiagLocal,

-                                                        RigidBodyA.InverseMass,

-                                                        RigidBodyB.InvInertiaDiagLocal,

-                                                        RigidBodyB.InverseMass);

-                                        MathHelper.SetElement(ref normal, i, 0);

-                                }

-                        }

-

-                        //calculate two perpendicular jointAxis, orthogonal to hingeAxis

-                        //these two jointAxis require equal angular velocities for both bodies

-                        //this is unused for now, it's a todo

-                        Vector3 jointAxisALocal = new Vector3();

-                        Vector3 jointAxisBLocal = new Vector3();

-                        MathHelper.PlaneSpace1(_axisInA, ref jointAxisALocal, ref jointAxisBLocal);

-

-                        Vector3 jointAxisA = MathHelper.TransformNormal(jointAxisALocal, RigidBodyA.CenterOfMassTransform);

-                        Vector3 jointAxisB = MathHelper.TransformNormal(jointAxisBLocal, RigidBodyA.CenterOfMassTransform);

-                        Vector3 hingeAxisWorld = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform);

-

-                        _jacAng[0] = new JacobianEntry(jointAxisA,

-                                MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),

-                                MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),

-                                RigidBodyA.InvInertiaDiagLocal,

-                                RigidBodyB.InvInertiaDiagLocal);

-

-                        _jacAng[1] = new JacobianEntry(jointAxisB,

-                                MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),

-                                MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),

-                                RigidBodyA.InvInertiaDiagLocal,

-                                RigidBodyB.InvInertiaDiagLocal);

-

-                        _jacAng[2] = new JacobianEntry(hingeAxisWorld,

-                                MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),

-                                MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),

-                                RigidBodyA.InvInertiaDiagLocal,

-                                RigidBodyB.InvInertiaDiagLocal);

-                }

-

-                public override void SolveConstraint(float timeStep)

-                {

-                        Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform);

-                        Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform);

-

-                        Vector3 normal = new Vector3(0, 0, 0);

-                        float tau = 0.3f;

-                        float damping = 1f;

-

-                        //linear part

-                        if (!_angularOnly)

-                        {

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

-                                {

-                                        if (i == 0)

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

-                                        else if (i == 1)

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

-                                        else

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

-

-                                        float jacDiagABInv = 1f / _jac[i].Diagonal;

-

-                                        Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;

-                                        Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;

-

-                                        Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1);

-                                        Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2);

-                                        Vector3 vel = vel1 - vel2;

-                                        float rel_vel;

-                                        rel_vel = Vector3.Dot(normal, vel);

-                                        //positional error (zeroth order error)

-                                        float depth = -Vector3.Dot(pivotAInW - pivotBInW, normal); //this is the error projected on the normal

-                                        float impulse = depth * tau / timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping;

-                                        AppliedImpulse += impulse;

-                                        Vector3 impulse_vector = normal * impulse;

-                                        RigidBodyA.ApplyImpulse(impulse_vector, pivotAInW - RigidBodyA.CenterOfMassPosition);

-                                        RigidBodyB.ApplyImpulse(-impulse_vector, pivotBInW - RigidBodyB.CenterOfMassPosition);

-                                }

-                        }

-                        //solve angular part

-                        // get axes in world space

-                        Vector3 axisA = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform);

-                        Vector3 axisB = MathHelper.TransformNormal(_axisInB, RigidBodyB.CenterOfMassTransform);

-

-                        Vector3 angVelA = RigidBodyA.AngularVelocity;

-                        Vector3 angVelB = RigidBodyB.AngularVelocity;

-                        Vector3 angVelAroundHingeAxisA = axisA * Vector3.Dot(axisA, angVelA);

-                        Vector3 angVelAroundHingeAxisB = axisB * Vector3.Dot(axisB, angVelB);

-

-                        Vector3 angAOrthog = angVelA - angVelAroundHingeAxisA;

-                        Vector3 angBOrthog = angVelB - angVelAroundHingeAxisB;

-                        Vector3 velrelOrthog = angAOrthog - angBOrthog;

-

-                        //solve angular velocity correction

-                        float relaxation = 1f;

-                        float len = velrelOrthog.Length();

-                        if (len > 0.00001f)

-                        {

-                                Vector3 normal2 = Vector3.Normalize(velrelOrthog);

-                                float denom = RigidBodyA.ComputeAngularImpulseDenominator(normal2) +

-                                        RigidBodyB.ComputeAngularImpulseDenominator(normal2);

-                                // scale for mass and relaxation

-                                velrelOrthog *= (1f / denom) * 0.9f;

-                        }

-

-                        //solve angular positional correction

-                        Vector3 angularError = -Vector3.Cross(axisA, axisB) * (1f / timeStep);

-                        float len2 = angularError.Length();

-                        if (len2 > 0.00001f)

-                        {

-                                Vector3 normal2 = Vector3.Normalize(angularError);

-                                float denom2 = RigidBodyA.ComputeAngularImpulseDenominator(normal2) +

-                                                RigidBodyB.ComputeAngularImpulseDenominator(normal2);

-                                angularError *= (1f / denom2) * relaxation;

-                        }

-

-                        RigidBodyA.ApplyTorqueImpulse(-velrelOrthog + angularError);

-                        RigidBodyB.ApplyTorqueImpulse(velrelOrthog - angularError);

-

-                        //apply motor

-                        if (_enableAngularMotor)

-                        {

-                                //todo: add limits too

-                                Vector3 angularLimit = Vector3.Zero;

-

-                                Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;

-                                float projRelVel = Vector3.Dot(velrel, axisA);

-

-                                float desiredMotorVel = _motorTargetVelocity;

-                                float motorRelvel = desiredMotorVel - projRelVel;

-

-                                float denom3 = RigidBodyA.ComputeAngularImpulseDenominator(axisA) +

-                                                RigidBodyB.ComputeAngularImpulseDenominator(axisA);

-

-                                float unclippedMotorImpulse = (1f / denom3) * motorRelvel;

-                                //todo: should clip against accumulated impulse

-                                float clippedMotorImpulse = unclippedMotorImpulse > _maxMotorImpulse ? _maxMotorImpulse : unclippedMotorImpulse;

-                                clippedMotorImpulse = clippedMotorImpulse < -_maxMotorImpulse ? -_maxMotorImpulse : clippedMotorImpulse;

-                                Vector3 motorImp = clippedMotorImpulse * axisA;

-

-                                RigidBodyA.ApplyTorqueImpulse(motorImp + angularLimit);

-                                RigidBodyB.ApplyTorqueImpulse(-motorImp - angularLimit);

-                        }

-                }

-

-                public void EnableAngularMotor(bool enableMotor, float targetVelocity, float maxMotorImpulse)

-                {

-                        _enableAngularMotor = enableMotor;

-                        _motorTargetVelocity = targetVelocity;

-                        _maxMotorImpulse = maxMotorImpulse;

-                }

-

-                public void UpdateRHS(float timeStep)

-                {

-                }

-        }

-}

+/*
+  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.Dynamics
+{
+        /// <summary>
+        /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
+        /// axis defines the orientation of the hinge axis
+        /// </summary>
+        public class HingeConstraint : TypedConstraint
+        {
+                private JacobianEntry[] _jac = new JacobianEntry[3]; //3 orthogonal linear constraints
+                private JacobianEntry[] _jacAng = new JacobianEntry[3]; //2 orthogonal angular constraints + 1 for limit/motor
+
+                private Vector3 _pivotInA;
+                private Vector3 _pivotInB;
+                private Vector3 _axisInA;
+                private Vector3 _axisInB;
+
+                private bool _angularOnly;
+
+                private float _motorTargetVelocity;
+                private float _maxMotorImpulse;
+                private bool _enableAngularMotor;
+
+                public HingeConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB, Vector3 axisInA, Vector3 axisInB)
+                        : base(rbA, rbB)
+                {
+                        _pivotInA = pivotInA;
+                        _pivotInB = pivotInB;
+                        _axisInA = axisInA;
+                        _axisInB = -axisInB;
+                        _angularOnly = false;
+                }
+
+                public HingeConstraint(RigidBody rbA, Vector3 pivotInA, Vector3 axisInA)
+                        : base(rbA)
+                {
+                        _pivotInA = pivotInA;
+                        _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, pivotInA);
+                        _axisInA = axisInA;
+                        //fixed axis in worldspace
+                        _axisInB = MathHelper.TransformNormal(-axisInA, rbA.CenterOfMassTransform);
+                        _angularOnly = false;
+                }
+
+                public HingeConstraint() { }
+
+                public bool AngularOnly { set { _angularOnly = value; } }
+
+                public override void BuildJacobian()
+                {
+                        AppliedImpulse = 0f;
+
+                        Vector3 normal = new Vector3();
+
+                        if (!_angularOnly)
+                        {
+                                for (int i = 0; i < 3; i++)
+                                {
+                                        MathHelper.SetElement(ref normal, i, 1);
+                                        _jac[i] = new JacobianEntry(
+                                                        MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
+                                                        MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
+                                                        MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition,
+                                                        MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition,
+                                                        normal,
+                                                        RigidBodyA.InvInertiaDiagLocal,
+                                                        RigidBodyA.InverseMass,
+                                                        RigidBodyB.InvInertiaDiagLocal,
+                                                        RigidBodyB.InverseMass);
+                                        MathHelper.SetElement(ref normal, i, 0);
+                                }
+                        }
+
+                        //calculate two perpendicular jointAxis, orthogonal to hingeAxis
+                        //these two jointAxis require equal angular velocities for both bodies
+                        //this is unused for now, it's a todo
+                        Vector3 jointAxisALocal = new Vector3();
+                        Vector3 jointAxisBLocal = new Vector3();
+                        MathHelper.PlaneSpace1(_axisInA, ref jointAxisALocal, ref jointAxisBLocal);
+
+                        Vector3 jointAxisA = MathHelper.TransformNormal(jointAxisALocal, RigidBodyA.CenterOfMassTransform);
+                        Vector3 jointAxisB = MathHelper.TransformNormal(jointAxisBLocal, RigidBodyA.CenterOfMassTransform);
+                        Vector3 hingeAxisWorld = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform);
+
+                        _jacAng[0] = new JacobianEntry(jointAxisA,
+                                MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
+                                MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
+                                RigidBodyA.InvInertiaDiagLocal,
+                                RigidBodyB.InvInertiaDiagLocal);
+
+                        _jacAng[1] = new JacobianEntry(jointAxisB,
+                                MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
+                                MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
+                                RigidBodyA.InvInertiaDiagLocal,
+                                RigidBodyB.InvInertiaDiagLocal);
+
+                        _jacAng[2] = new JacobianEntry(hingeAxisWorld,
+                                MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
+                                MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
+                                RigidBodyA.InvInertiaDiagLocal,
+                                RigidBodyB.InvInertiaDiagLocal);
+                }
+
+                public override void SolveConstraint(float timeStep)
+                {
+                        Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform);
+                        Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform);
+
+                        Vector3 normal = new Vector3(0, 0, 0);
+                        float tau = 0.3f;
+                        float damping = 1f;
+
+                        //linear part
+                        if (!_angularOnly)
+                        {
+                                for (int i = 0; i < 3; i++)
+                                {
+                                        if (i == 0)
+                                                normal = new Vector3(1, 0, 0);
+                                        else if (i == 1)
+                                                normal = new Vector3(0, 1, 0);
+                                        else
+                                                normal = new Vector3(0, 0, 1);
+
+                                        float jacDiagABInv = 1f / _jac[i].Diagonal;
+
+                                        Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
+                                        Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;
+
+                                        Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1);
+                                        Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2);
+                                        Vector3 vel = vel1 - vel2;
+                                        float rel_vel;
+                                        rel_vel = Vector3.Dot(normal, vel);
+                                        //positional error (zeroth order error)
+                                        float depth = -Vector3.Dot(pivotAInW - pivotBInW, normal); //this is the error projected on the normal
+                                        float impulse = depth * tau / timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping;
+                                        AppliedImpulse += impulse;
+                                        Vector3 impulse_vector = normal * impulse;
+                                        RigidBodyA.ApplyImpulse(impulse_vector, pivotAInW - RigidBodyA.CenterOfMassPosition);
+                                        RigidBodyB.ApplyImpulse(-impulse_vector, pivotBInW - RigidBodyB.CenterOfMassPosition);
+                                }
+                        }
+                        //solve angular part
+                        // get axes in world space
+                        Vector3 axisA = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform);
+                        Vector3 axisB = MathHelper.TransformNormal(_axisInB, RigidBodyB.CenterOfMassTransform);
+
+                        Vector3 angVelA = RigidBodyA.AngularVelocity;
+                        Vector3 angVelB = RigidBodyB.AngularVelocity;
+                        Vector3 angVelAroundHingeAxisA = axisA * Vector3.Dot(axisA, angVelA);
+                        Vector3 angVelAroundHingeAxisB = axisB * Vector3.Dot(axisB, angVelB);
+
+                        Vector3 angAOrthog = angVelA - angVelAroundHingeAxisA;
+                        Vector3 angBOrthog = angVelB - angVelAroundHingeAxisB;
+                        Vector3 velrelOrthog = angAOrthog - angBOrthog;
+
+                        //solve angular velocity correction
+                        float relaxation = 1f;
+                        float len = velrelOrthog.Length();
+                        if (len > 0.00001f)
+                        {
+                                Vector3 normal2 = Vector3.Normalize(velrelOrthog);
+                                float denom = RigidBodyA.ComputeAngularImpulseDenominator(normal2) +
+                                        RigidBodyB.ComputeAngularImpulseDenominator(normal2);
+                                // scale for mass and relaxation
+                                velrelOrthog *= (1f / denom) * 0.9f;
+                        }
+
+                        //solve angular positional correction
+                        Vector3 angularError = -Vector3.Cross(axisA, axisB) * (1f / timeStep);
+                        float len2 = angularError.Length();
+                        if (len2 > 0.00001f)
+                        {
+                                Vector3 normal2 = Vector3.Normalize(angularError);
+                                float denom2 = RigidBodyA.ComputeAngularImpulseDenominator(normal2) +
+                                                RigidBodyB.ComputeAngularImpulseDenominator(normal2);
+                                angularError *= (1f / denom2) * relaxation;
+                        }
+
+                        RigidBodyA.ApplyTorqueImpulse(-velrelOrthog + angularError);
+                        RigidBodyB.ApplyTorqueImpulse(velrelOrthog - angularError);
+
+                        //apply motor
+                        if (_enableAngularMotor)
+                        {
+                                //todo: add limits too
+                                Vector3 angularLimit = Vector3.Zero;
+
+                                Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
+                                float projRelVel = Vector3.Dot(velrel, axisA);
+
+                                float desiredMotorVel = _motorTargetVelocity;
+                                float motorRelvel = desiredMotorVel - projRelVel;
+
+                                float denom3 = RigidBodyA.ComputeAngularImpulseDenominator(axisA) +
+                                                RigidBodyB.ComputeAngularImpulseDenominator(axisA);
+
+                                float unclippedMotorImpulse = (1f / denom3) * motorRelvel;
+                                //todo: should clip against accumulated impulse
+                                float clippedMotorImpulse = unclippedMotorImpulse > _maxMotorImpulse ? _maxMotorImpulse : unclippedMotorImpulse;
+                                clippedMotorImpulse = clippedMotorImpulse < -_maxMotorImpulse ? -_maxMotorImpulse : clippedMotorImpulse;
+                                Vector3 motorImp = clippedMotorImpulse * axisA;
+
+                                RigidBodyA.ApplyTorqueImpulse(motorImp + angularLimit);
+                                RigidBodyB.ApplyTorqueImpulse(-motorImp - angularLimit);
+                        }
+                }
+
+                public void EnableAngularMotor(bool enableMotor, float targetVelocity, float maxMotorImpulse)
+                {
+                        _enableAngularMotor = enableMotor;
+                        _motorTargetVelocity = targetVelocity;
+                        _maxMotorImpulse = maxMotorImpulse;
+                }
+
+                public void UpdateRHS(float timeStep)
+                {
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs
index de25225..a83eaa5 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs
@@ -1,32 +1,32 @@
-/*

-  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;

-

-namespace XnaDevRu.BulletX.Dynamics

-{

-        public interface IConstraintSolver

-        {

-                float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo info, IDebugDraw debugDrawer);

-        }

-}

+/*
+  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;
+
+namespace XnaDevRu.BulletX.Dynamics
+{
+        public interface IConstraintSolver
+        {
+                float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo info, IDebugDraw debugDrawer);
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs
index 3b8d8d8..6249998 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs
@@ -1,155 +1,155 @@
-/*

-  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.Dynamics

-{

-        /// <summary>

-        /// Jacobian entry is an abstraction that allows to describe constraints

-        /// it can be used in combination with a constraint solver

-        /// Can be used to relate the effect of an impulse to the constraint error

-        /// </summary>

-        public class JacobianEntry

-        {

-                private Vector3 _linearJointAxis;

-                private Vector3 _aJ;

-                private Vector3 _bJ;

-                private Vector3 _0MinvJt;

-                private Vector3 _1MinvJt;

-                private float _adiag;

-

-                public JacobianEntry() { }

-

-                //constraint between two different rigidbodies

-                public JacobianEntry(

-                        Matrix world2A,

-                        Matrix world2B,

-                        Vector3 relPosA, Vector3 relPosB,

-                        Vector3 jointAxis,

-                        Vector3 inertiaInvA,

-                        float massInvA,

-                        Vector3 inertiaInvB,

-                        float massInvB)

-                {

-                        _linearJointAxis = jointAxis;

-                        _aJ = Vector3.TransformNormal(Vector3.Cross(relPosA, _linearJointAxis), world2A);

-                        _bJ = Vector3.TransformNormal(Vector3.Cross(relPosB, -_linearJointAxis), world2B);

-                        _0MinvJt = inertiaInvA * _aJ;

-                        _1MinvJt = inertiaInvB * _bJ;

-                        _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ) + massInvB + Vector3.Dot(_1MinvJt, _bJ);

-

-                        if (_adiag <= 0.0f)

-                                throw new BulletException();

-                }

-

-                //angular constraint between two different rigidbodies

-                public JacobianEntry(Vector3 jointAxis,

-                        Matrix world2A,

-                        Matrix world2B,

-                        Vector3 inertiaInvA,

-                        Vector3 inertiaInvB)

-                {

-                        _linearJointAxis = new Vector3();

-                        _aJ = Vector3.TransformNormal(jointAxis, world2A);

-                        _bJ = Vector3.TransformNormal(-jointAxis, world2B);

-                        _0MinvJt = inertiaInvA * _aJ;

-                        _1MinvJt = inertiaInvB * _bJ;

-                        _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ);

-

-                        if (_adiag <= 0.0f)

-                                throw new BulletException();

-                }

-

-                //angular constraint between two different rigidbodies

-                public JacobianEntry(Vector3 axisInA,

-                        Vector3 axisInB,

-                        Vector3 inertiaInvA,

-                        Vector3 inertiaInvB)

-                {

-                        _linearJointAxis = new Vector3();

-                        _aJ = axisInA;

-                        _bJ = -axisInB;

-                        _0MinvJt = inertiaInvA * _aJ;

-                        _1MinvJt = inertiaInvB * _bJ;

-                        _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ);

-

-                        if (_adiag <= 0.0f)

-                                throw new BulletException();

-                }

-

-                //constraint on one rigidbody

-                public JacobianEntry(

-                        Matrix world2A,

-                        Vector3 rel_pos1, Vector3 rel_pos2,

-                        Vector3 jointAxis,

-                        Vector3 inertiaInvA,

-                        float massInvA)

-                {

-                        _linearJointAxis = jointAxis;

-                        _aJ = Vector3.TransformNormal(Vector3.Cross(rel_pos1, jointAxis), world2A);

-                        _bJ = Vector3.TransformNormal(Vector3.Cross(rel_pos2, -jointAxis), world2A);

-                        _0MinvJt = inertiaInvA * _aJ;

-                        _1MinvJt = new Vector3();

-                        _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ);

-

-                        if (_adiag <= 0.0f)

-                                throw new BulletException();

-                }

-

-                public float Diagonal { get { return _adiag; } }

-

-                // for two constraints on the same rigidbody (for example vehicle friction)

-                public float GetNonDiagonal(JacobianEntry jacB, float massInvA)

-                {

-                        float lin = massInvA * Vector3.Dot(_linearJointAxis, jacB._linearJointAxis);

-                        float ang = Vector3.Dot(_0MinvJt, jacB._aJ);

-                        return lin + ang;

-                }

-

-                // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)

-                public float GetNonDiagonal(JacobianEntry jacB, float massInvA, float massInvB)

-                {

-                        Vector3 lin = _linearJointAxis * jacB._linearJointAxis;

-                        Vector3 ang0 = _0MinvJt * jacB._aJ;

-                        Vector3 ang1 = _1MinvJt * jacB._bJ;

-                        Vector3 lin0 = massInvA * lin;

-                        Vector3 lin1 = massInvB * lin;

-                        Vector3 sum = ang0 + ang1 + lin0 + lin1;

-                        return sum.X + sum.Y + sum.Z;

-                }

-

-                public float GetRelativeVelocity(Vector3 linvelA, Vector3 angvelA, Vector3 linvelB, Vector3 angvelB)

-                {

-                        Vector3 linrel = linvelA - linvelB;

-                        Vector3 angvela = angvelA * _aJ;

-                        Vector3 angvelb = angvelB * _bJ;

-                        linrel *= _linearJointAxis;

-                        angvela += angvelb;

-                        angvela += linrel;

-                        float rel_vel2 = angvela.X + angvela.Y + angvela.Z;

-                        return rel_vel2 + float.Epsilon;

-                }

-        }

-}

+/*
+  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.Dynamics
+{
+        /// <summary>
+        /// Jacobian entry is an abstraction that allows to describe constraints
+        /// it can be used in combination with a constraint solver
+        /// Can be used to relate the effect of an impulse to the constraint error
+        /// </summary>
+        public class JacobianEntry
+        {
+                private Vector3 _linearJointAxis;
+                private Vector3 _aJ;
+                private Vector3 _bJ;
+                private Vector3 _0MinvJt;
+                private Vector3 _1MinvJt;
+                private float _adiag;
+
+                public JacobianEntry() { }
+
+                //constraint between two different rigidbodies
+                public JacobianEntry(
+                        Matrix world2A,
+                        Matrix world2B,
+                        Vector3 relPosA, Vector3 relPosB,
+                        Vector3 jointAxis,
+                        Vector3 inertiaInvA,
+                        float massInvA,
+                        Vector3 inertiaInvB,
+                        float massInvB)
+                {
+                        _linearJointAxis = jointAxis;
+                        _aJ = Vector3.TransformNormal(Vector3.Cross(relPosA, _linearJointAxis), world2A);
+                        _bJ = Vector3.TransformNormal(Vector3.Cross(relPosB, -_linearJointAxis), world2B);
+                        _0MinvJt = inertiaInvA * _aJ;
+                        _1MinvJt = inertiaInvB * _bJ;
+                        _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ) + massInvB + Vector3.Dot(_1MinvJt, _bJ);
+
+                        if (_adiag <= 0.0f)
+                                throw new BulletException();
+                }
+
+                //angular constraint between two different rigidbodies
+                public JacobianEntry(Vector3 jointAxis,
+                        Matrix world2A,
+                        Matrix world2B,
+                        Vector3 inertiaInvA,
+                        Vector3 inertiaInvB)
+                {
+                        _linearJointAxis = new Vector3();
+                        _aJ = Vector3.TransformNormal(jointAxis, world2A);
+                        _bJ = Vector3.TransformNormal(-jointAxis, world2B);
+                        _0MinvJt = inertiaInvA * _aJ;
+                        _1MinvJt = inertiaInvB * _bJ;
+                        _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ);
+
+                        if (_adiag <= 0.0f)
+                                throw new BulletException();
+                }
+
+                //angular constraint between two different rigidbodies
+                public JacobianEntry(Vector3 axisInA,
+                        Vector3 axisInB,
+                        Vector3 inertiaInvA,
+                        Vector3 inertiaInvB)
+                {
+                        _linearJointAxis = new Vector3();
+                        _aJ = axisInA;
+                        _bJ = -axisInB;
+                        _0MinvJt = inertiaInvA * _aJ;
+                        _1MinvJt = inertiaInvB * _bJ;
+                        _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ);
+
+                        if (_adiag <= 0.0f)
+                                throw new BulletException();
+                }
+
+                //constraint on one rigidbody
+                public JacobianEntry(
+                        Matrix world2A,
+                        Vector3 rel_pos1, Vector3 rel_pos2,
+                        Vector3 jointAxis,
+                        Vector3 inertiaInvA,
+                        float massInvA)
+                {
+                        _linearJointAxis = jointAxis;
+                        _aJ = Vector3.TransformNormal(Vector3.Cross(rel_pos1, jointAxis), world2A);
+                        _bJ = Vector3.TransformNormal(Vector3.Cross(rel_pos2, -jointAxis), world2A);
+                        _0MinvJt = inertiaInvA * _aJ;
+                        _1MinvJt = new Vector3();
+                        _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ);
+
+                        if (_adiag <= 0.0f)
+                                throw new BulletException();
+                }
+
+                public float Diagonal { get { return _adiag; } }
+
+                // for two constraints on the same rigidbody (for example vehicle friction)
+                public float GetNonDiagonal(JacobianEntry jacB, float massInvA)
+                {
+                        float lin = massInvA * Vector3.Dot(_linearJointAxis, jacB._linearJointAxis);
+                        float ang = Vector3.Dot(_0MinvJt, jacB._aJ);
+                        return lin + ang;
+                }
+
+                // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
+                public float GetNonDiagonal(JacobianEntry jacB, float massInvA, float massInvB)
+                {
+                        Vector3 lin = _linearJointAxis * jacB._linearJointAxis;
+                        Vector3 ang0 = _0MinvJt * jacB._aJ;
+                        Vector3 ang1 = _1MinvJt * jacB._bJ;
+                        Vector3 lin0 = massInvA * lin;
+                        Vector3 lin1 = massInvB * lin;
+                        Vector3 sum = ang0 + ang1 + lin0 + lin1;
+                        return sum.X + sum.Y + sum.Z;
+                }
+
+                public float GetRelativeVelocity(Vector3 linvelA, Vector3 angvelA, Vector3 linvelB, Vector3 angvelB)
+                {
+                        Vector3 linrel = linvelA - linvelB;
+                        Vector3 angvela = angvelA * _aJ;
+                        Vector3 angvelb = angvelB * _bJ;
+                        linrel *= _linearJointAxis;
+                        angvela += angvelb;
+                        angvela += linrel;
+                        float rel_vel2 = angvela.X + angvela.Y + angvela.Z;
+                        return rel_vel2 + float.Epsilon;
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs
index d4206ea..8ffd3d5 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs
@@ -1,157 +1,157 @@
-/*

-  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.Dynamics

-{

-        public class ConstraintSetting

-        {

-                private float _tau, _damping;

-

-                public ConstraintSetting()

-                {

-                        _tau = 0.3f;

-                        _damping = 1.0f;

-                }

-

-                public float Damping

-                {

-                        get { return _damping; }

-                        set { _damping = value; }

-                }

-

-                public float Tau

-                {

-                        get { return _tau; }

-                        set { _tau = value; }

-                }

-        }

-

-        public class Point2PointConstraint : TypedConstraint

-        {

-                private JacobianEntry[] _jacobian;

-                private Vector3 _pivotInA, _pivotInB;

-

-                private ConstraintSetting _setting = new ConstraintSetting();

-

-                public Point2PointConstraint()

-                {

-                        _jacobian = new JacobianEntry[3];

-                }

-

-                public Point2PointConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB)

-                        : base(rbA, rbB)

-                {

-                        _jacobian = new JacobianEntry[3];

-

-                        _pivotInA = pivotInA;

-                        _pivotInB = pivotInB;

-                }

-

-                public Point2PointConstraint(RigidBody rbA, Vector3 pivotInA)

-                        : base(rbA)

-                {

-                        _jacobian = new JacobianEntry[3];

-

-                        _pivotInA = pivotInA;

-                        _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, _pivotInA);

-                }

-

-                public ConstraintSetting Settings { get { return _setting; } set { _setting = value; } }

-

-                public Vector3 PivotInA

-                {

-                        set

-                        {

-                                _pivotInA = value;

-                        }

-                }

-

-                public Vector3 PivotInB

-                {

-                        set

-                        {

-                                _pivotInB = value;

-                        }

-                }

-

-                public override void BuildJacobian()

-                {

-                        Vector3 normal = new Vector3();

-

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

-                        {

-                                MathHelper.SetElement(ref normal, i, 1);

-                                _jacobian[i] = new JacobianEntry(

-                                        MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),

-                                        MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),

-                                        MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition,

-                                        MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition,

-                                        normal,

-                                        RigidBodyA.InvInertiaDiagLocal,

-                                        RigidBodyA.InverseMass,

-                                        RigidBodyB.InvInertiaDiagLocal,

-                                        RigidBodyB.InverseMass

-                                );

-                                MathHelper.SetElement(ref normal, i, 0);

-                        }

-                }

-

-                public override void SolveConstraint(float timeStep)

-                {

-                        Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform);

-                        Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform);

-

-                        Vector3 normal = new Vector3();

-

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

-                        {

-                                MathHelper.SetElement(ref normal, i, 1);

-

-                                float jacDiagABInv = 1.0f / _jacobian[i].Diagonal;

-

-                                Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;

-                                Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;

-

-                                Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1);

-                                Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2);

-

-                                Vector3 vel = vel1 - vel2;

-

-                                float rel_vel = Vector3.Dot(normal, vel);

-                                float depth = -Vector3.Dot((pivotAInW - pivotBInW), normal);

-

-                                float impulse = depth * _setting.Tau / timeStep * jacDiagABInv - _setting.Damping * rel_vel * jacDiagABInv;

-                                AppliedImpulse += impulse;

-                                Vector3 impulseVector = normal * impulse;

-

-                                RigidBodyA.ApplyImpulse(impulseVector, pivotAInW - RigidBodyA.CenterOfMassPosition);

-                                RigidBodyB.ApplyImpulse(-impulseVector, pivotBInW - RigidBodyB.CenterOfMassPosition);

-

-                                MathHelper.SetElement(ref normal, i, 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.Dynamics
+{
+        public class ConstraintSetting
+        {
+                private float _tau, _damping;
+
+                public ConstraintSetting()
+                {
+                        _tau = 0.3f;
+                        _damping = 1.0f;
+                }
+
+                public float Damping
+                {
+                        get { return _damping; }
+                        set { _damping = value; }
+                }
+
+                public float Tau
+                {
+                        get { return _tau; }
+                        set { _tau = value; }
+                }
+        }
+
+        public class Point2PointConstraint : TypedConstraint
+        {
+                private JacobianEntry[] _jacobian;
+                private Vector3 _pivotInA, _pivotInB;
+
+                private ConstraintSetting _setting = new ConstraintSetting();
+
+                public Point2PointConstraint()
+                {
+                        _jacobian = new JacobianEntry[3];
+                }
+
+                public Point2PointConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB)
+                        : base(rbA, rbB)
+                {
+                        _jacobian = new JacobianEntry[3];
+
+                        _pivotInA = pivotInA;
+                        _pivotInB = pivotInB;
+                }
+
+                public Point2PointConstraint(RigidBody rbA, Vector3 pivotInA)
+                        : base(rbA)
+                {
+                        _jacobian = new JacobianEntry[3];
+
+                        _pivotInA = pivotInA;
+                        _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, _pivotInA);
+                }
+
+                public ConstraintSetting Settings { get { return _setting; } set { _setting = value; } }
+
+                public Vector3 PivotInA
+                {
+                        set
+                        {
+                                _pivotInA = value;
+                        }
+                }
+
+                public Vector3 PivotInB
+                {
+                        set
+                        {
+                                _pivotInB = value;
+                        }
+                }
+
+                public override void BuildJacobian()
+                {
+                        Vector3 normal = new Vector3();
+
+                        for (int i = 0; i < 3; i++)
+                        {
+                                MathHelper.SetElement(ref normal, i, 1);
+                                _jacobian[i] = new JacobianEntry(
+                                        MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
+                                        MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
+                                        MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition,
+                                        MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition,
+                                        normal,
+                                        RigidBodyA.InvInertiaDiagLocal,
+                                        RigidBodyA.InverseMass,
+                                        RigidBodyB.InvInertiaDiagLocal,
+                                        RigidBodyB.InverseMass
+                                );
+                                MathHelper.SetElement(ref normal, i, 0);
+                        }
+                }
+
+                public override void SolveConstraint(float timeStep)
+                {
+                        Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform);
+                        Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform);
+
+                        Vector3 normal = new Vector3();
+
+                        for (int i = 0; i < 3; i++)
+                        {
+                                MathHelper.SetElement(ref normal, i, 1);
+
+                                float jacDiagABInv = 1.0f / _jacobian[i].Diagonal;
+
+                                Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
+                                Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;
+
+                                Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1);
+                                Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2);
+
+                                Vector3 vel = vel1 - vel2;
+
+                                float rel_vel = Vector3.Dot(normal, vel);
+                                float depth = -Vector3.Dot((pivotAInW - pivotBInW), normal);
+
+                                float impulse = depth * _setting.Tau / timeStep * jacDiagABInv - _setting.Damping * rel_vel * jacDiagABInv;
+                                AppliedImpulse += impulse;
+                                Vector3 impulseVector = normal * impulse;
+
+                                RigidBodyA.ApplyImpulse(impulseVector, pivotAInW - RigidBodyA.CenterOfMassPosition);
+                                RigidBodyB.ApplyImpulse(-impulseVector, pivotBInW - RigidBodyB.CenterOfMassPosition);
+
+                                MathHelper.SetElement(ref normal, i, 0);
+                        }
+                }
+        }
 }
\ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs
index 4d5e93d..f275550 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs
@@ -1,915 +1,915 @@
-/*

-  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.Dynamics

-{

-        [Flags]

-        public enum SolverMode

-        {

-                None = 0,

-                RandomizeOrder = 1,

-                FrictionSeperate = 2,

-                UseWarmstarting = 4,

-                CacheFriendly = 8,

-        }

-

-        public class SequentialImpulseConstraintSolver : IConstraintSolver

-        {

-                private static int _totalContactPoints = 0;

-

-                private SolverMode _solverMode;

-                private int _totalCpd = 0;

-                private ContactSolverFunc[,] _contactDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType];

-                private ContactSolverFunc[,] _frictionDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType];

-

-                private float _penetrationResolveFactor = 0.9f;

-                private List<SolverBody> _tmpSolverBodyPool = new List<SolverBody>();

-                private List<SolverConstraint> _tmpSolverConstraintPool = new List<SolverConstraint>();

-                private List<SolverConstraint> _tmpSolverFrictionConstraintPool = new List<SolverConstraint>();

-

-                private const int _sequentialImpulseMaxSolverPoints = 16384;

-                private static OrderIndex[] _order = new OrderIndex[SequentialImpulseMaxSolverPoints];

-                private static long _seed2 = 0;

-

-                public SequentialImpulseConstraintSolver()

-                {

-                        _solverMode = SolverMode.RandomizeOrder | SolverMode.CacheFriendly;

-                        PersistentManifold.ContactDestroyedCallback = MyContactDestroyedCallback;

-

-                        //initialize default friction/contact funcs

-                        int i, j;

-                        for (i = 0; i < (int)ContactSolverType.MaxContactSolverType; i++)

-                                for (j = 0; j < (int)ContactSolverType.MaxContactSolverType; j++)

-                                {

-

-                                        _contactDispatch[i, j] = ContactConstraint.ResolveSingleCollision;

-                                        _frictionDispatch[i, j] = ContactConstraint.ResolveSingleFriction;

-                                }

-                }

-

-                public SolverMode SolverMode { get { return _solverMode; } set { _solverMode = value; } }

-                public static int SequentialImpulseMaxSolverPoints { get { return _sequentialImpulseMaxSolverPoints; } }

-                protected static OrderIndex[] Order { get { return _order; } set { _order = value; } }

-                public static long RandSeed { get { return _seed2; } set { _seed2 = value; } }

-

-                ///<summary>

-                /// Advanced: Override the default contact solving function for contacts, for certain types of rigidbody

-                /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType

-                ///</summary>

-                public void SetContactSolverFunc(ContactSolverFunc func, int typeA, int typeB)

-                {

-                        _contactDispatch[typeA, typeB] = func;

-                }

-

-                /// <summary>

-                /// Advanced: Override the default friction solving function for contacts, for certain types of rigidbody

-                /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType

-                ///</summary>

-                public void SetFrictionSolverFunc(ContactSolverFunc func, int typeA, int typeB)

-                {

-                        _frictionDispatch[typeA, typeB] = func;

-                }

-

-                protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)

-                {

-                        float maxImpulse = 0;

-

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

-                        if (cp.Distance <= 0)

-                        {

-                                if (iter == 0)

-                                        if(debugDraw != null)

-                                                debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);

-

-                                ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;

-                                float impulse = cpd.ContactSolverFunc(

-                                        bodyA, bodyB,

-                                        cp,

-                                        info);

-

-                                if (maxImpulse < impulse)

-                                        maxImpulse = impulse;

-                        }

-                        return maxImpulse;

-                }

-

-                protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter)

-                {

-                        return Solve(bodyA, bodyB, cp, info, iter, null);

-                }

-

-                protected float SolveCombinedContactFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)

-                {

-                        float maxImpulse = 0;

-

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

-                        if (cp.Distance <= 0)

-                        {

-                                if (iter == 0)

-                                        if (debugDraw != null)

-                                                debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);

-

-                                float impulse = ContactConstraint.ResolveSingleCollisionCombined(

-                                        bodyA, bodyB,

-                                        cp,

-                                        info);

-

-                                if (maxImpulse < impulse)

-                                        maxImpulse = impulse;

-                        }

-                        return maxImpulse;

-                }

-

-                protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)

-                {

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

-

-                        if (cp.Distance <= 0)

-                        {

-

-                                ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;

-                                cpd.FrictionSolverFunc(

-                                        bodyA, bodyB,

-                                        cp,

-                                        info);

-                        }

-                        return 0;

-                }

-

-                protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info)

-                {

-                        RigidBody body0 = manifold.BodyA as RigidBody;

-                        RigidBody body1 = manifold.BodyB as RigidBody;

-

-

-                        //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop

-                        {

-                                manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform);

-

-                                int numpoints = manifold.ContactsCount;

-

-                                _totalContactPoints += numpoints;

-

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

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

-                                {

-                                        ManifoldPoint cp = manifold.GetContactPoint(i);

-                                        if (cp.Distance <= 0)

-                                        {

-                                                Vector3 pos1 = cp.PositionWorldOnA;

-                                                Vector3 pos2 = cp.PositionWorldOnB;

-

-                                                Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition;

-                                                Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition;

-

-

-                                                //this jacobian entry is re-used for all iterations

-                                                JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform),

-                                                        MatrixOperations.Transpose(body1.CenterOfMassTransform),

-                                                        rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass,

-                                                        body1.InvInertiaDiagLocal, body1.InverseMass);

-

-                                                float jacDiagAB = jac.Diagonal;

-

-                                                ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;

-                                                if (cpd != null)

-                                                {

-                                                        //might be invalid

-                                                        cpd.PersistentLifeTime++;

-                                                        if (cpd.PersistentLifeTime != cp.LifeTime)

-                                                        {

-                                                                //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());

-                                                                cpd = new ConstraintPersistentData();

-                                                                cpd.PersistentLifeTime = cp.LifeTime;

-

-                                                        }

-                                                }

-                                                else

-                                                {

-

-                                                        cpd = new ConstraintPersistentData();

-                                                        _totalCpd++;

-                                                        //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);

-                                                        cp.UserPersistentData = cpd;

-                                                        cpd.PersistentLifeTime = cp.LifeTime;

-                                                        //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());

-

-                                                }

-                                                if (cpd == null)

-                                                        throw new BulletException();

-

-                                                cpd.JacDiagABInv = 1f / jacDiagAB;

-

-                                                //Dependent on Rigidbody A and B types, fetch the contact/friction response func

-                                                //perhaps do a similar thing for friction/restutution combiner funcs...

-

-                                                cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType];

-                                                cpd.ContactSolverFunc = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType];

-

-                                                Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1);

-                                                Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2);

-                                                Vector3 vel = vel1 - vel2;

-                                                float rel_vel;

-                                                rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel);

-

-                                                float combinedRestitution = cp.CombinedRestitution;

-

-                                                cpd.Penetration = cp.Distance;

-                                                cpd.Friction = cp.CombinedFriction;

-                                                cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution);

-                                                if (cpd.Restitution < 0f)

-                                                {

-                                                        cpd.Restitution = 0.0f;

-

-                                                };

-

-                                                //restitution and penetration work in same direction so

-                                                //rel_vel 

-

-                                                float penVel = -cpd.Penetration / info.TimeStep;

-

-                                                if (cpd.Restitution > penVel)

-                                                {

-                                                        cpd.Penetration = 0;

-                                                }

-

-

-                                                float relaxation = info.Damping;

-                                                if ((_solverMode & SolverMode.UseWarmstarting) != 0)

-                                                {

-                                                        cpd.AppliedImpulse *= relaxation;

-                                                }

-                                                else

-                                                {

-                                                        cpd.AppliedImpulse = 0f;

-                                                }

-

-                                                //for friction

-                                                cpd.PreviousAppliedImpulse = cpd.AppliedImpulse;

-

-                                                //re-calculate friction direction every frame, todo: check if this is really needed

-                                                Vector3 fwta = cpd.FrictionWorldTangentialA;

-                                                Vector3 fwtb = cpd.FrictionWorldTangentialB;

-                                                MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb);

-                                                cpd.FrictionWorldTangentialA = fwta;

-                                                cpd.FrictionWorldTangentialB = fwtb;

-

-                                                cpd.AccumulatedTangentImpulseA = 0;

-                                                cpd.AccumulatedTangentImpulseB = 0;

-                                                float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA);

-                                                float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA);

-                                                float denom = relaxation / (denom0 + denom1);

-                                                cpd.JacDiagABInvTangentA = denom;

-

-

-                                                denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB);

-                                                denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB);

-                                                denom = relaxation / (denom0 + denom1);

-                                                cpd.JacDiagABInvTangentB = denom;

-

-                                                Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse;

-

-                                                {

-                                                        Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);

-                                                        cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld);

-                                                        Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);

-                                                        cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld);

-                                                }

-                                                {

-                                                        Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA);

-                                                        cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld);

-                                                }

-                                                {

-                                                        Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB);

-                                                        cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld);

-                                                }

-                                                {

-                                                        Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA);

-                                                        cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld);

-                                                }

-                                                {

-                                                        Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB);

-                                                        cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld);

-                                                }

-

-

-                                                //apply previous frames impulse on both bodies

-                                                body0.ApplyImpulse(totalImpulse, rel_pos1);

-                                                body1.ApplyImpulse(-totalImpulse, rel_pos2);

-                                        }

-                                }

-                        }

-                }

-

-                private bool MyContactDestroyedCallback(object userPersistentData)

-                {

-                        if (userPersistentData == null)

-                                throw new BulletException();

-                        ConstraintPersistentData cpd = userPersistentData as ConstraintPersistentData;

-                        _totalCpd--;

-                        return true;

-                }

-

-                private float RestitutionCurve(float relVel, float restitution)

-                {

-                        float rest = restitution * -relVel;

-                        return rest;

-                }

-

-                //velocity + friction

-                //response  between two dynamic objects with friction

-                public virtual float ResolveSingleCollisionCombinedCacheFriendly(

-                        SolverBody bodyA,

-                        SolverBody bodyB,

-                        SolverConstraint contactConstraint,

-                        ContactSolverInfo solverInfo)

-                {

-                        float normalImpulse = 0;

-

-                        if (contactConstraint.Penetration < 0)

-                                return 0;

-

-                        float relVel;

-                        float velADotn = Vector3.Dot(contactConstraint.ContactNormal,bodyA.LinearVelocity)

-                                                + Vector3.Dot(contactConstraint.RelPosACrossNormal,bodyA.AngularVelocity);

-                        float velBDotn = Vector3.Dot(contactConstraint.ContactNormal,bodyB.LinearVelocity)

-                                                + Vector3.Dot(contactConstraint.RelPosBCrossNormal,bodyB.AngularVelocity);

-

-                        relVel = velADotn - velBDotn;

-

-                        float positionalError = contactConstraint.Penetration;

-                        float velocityError = contactConstraint.Restitution - relVel;// * damping;

-

-                        float penetrationImpulse = positionalError * contactConstraint.JacDiagABInv;

-                        float velocityImpulse = velocityError * contactConstraint.JacDiagABInv;

-                        normalImpulse = penetrationImpulse + velocityImpulse;

-

-                        // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse

-                        float oldNormalImpulse = contactConstraint.AppliedImpulse;

-                        float sum = oldNormalImpulse + normalImpulse;

-                        contactConstraint.AppliedImpulse = 0 > sum ? 0 : sum;

-

-                        float oldVelocityImpulse = contactConstraint.AppliedVelocityImpulse;

-                        float velocitySum = oldVelocityImpulse + velocityImpulse;

-                        contactConstraint.AppliedVelocityImpulse = 0 > velocitySum ? 0 : velocitySum;

-

-                        normalImpulse = contactConstraint.AppliedImpulse - oldNormalImpulse;

-

-                        if (bodyA.InvMass != 0)

-                        {

-                                bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass,

-                                        contactConstraint.AngularComponentA, normalImpulse);

-                        }

-                        if (bodyB.InvMass != 0)

-                        {

-                                bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass,

-                                        contactConstraint.AngularComponentB, -normalImpulse);

-                        }

-

-                        return normalImpulse;

-                }

-

-                public virtual float ResolveSingleFrictionCacheFriendly(

-                        SolverBody bodyA,

-                        SolverBody bodyB,

-                        SolverConstraint contactConstraint,

-                        ContactSolverInfo solverInfo,

-                        float appliedNormalImpulse)

-                {

-                        float combinedFriction = contactConstraint.Friction;

-                        float limit = appliedNormalImpulse * combinedFriction;

-

-                        if (appliedNormalImpulse > 0)

-                        //friction

-                        {

-                                float j1;

-                                {

-                                        float relVel;

-                                        float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity)

-                                                                + Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity);

-                                        float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity)

-                                                + Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity);

-                                        relVel = velADotn - velBDotn;

-

-                                        // calculate j that moves us to zero relative velocity

-                                        j1 = -relVel * contactConstraint.JacDiagABInv;

-                                        float oldTangentImpulse = contactConstraint.AppliedImpulse;

-                                        contactConstraint.AppliedImpulse = oldTangentImpulse + j1;

-

-                                        float test = contactConstraint.AppliedImpulse;

-                                        MathHelper.SetMin(ref test, limit);

-                                        MathHelper.SetMax(ref test, -limit);

-                                        contactConstraint.AppliedImpulse = test;

-

-                                        j1 = contactConstraint.AppliedImpulse - oldTangentImpulse;

-                                }

-

-                                if (bodyA.InvMass != 0)

-                                {

-                                        bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, j1);

-                                }

-                                if (bodyB.InvMass != 0)

-                                {

-                                        bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -j1);

-                                }

-                        }

-                        return 0;

-                }

-

-                public virtual float SolveGroupCacheFriendly(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer)

-                {

-                        if (constraints.Count + numManifolds == 0)

-                        {

-                                return 0;

-                        }

-

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

-                        {

-                                PersistentManifold manifold = manifolds[i];

-                                RigidBody rbA = (RigidBody)manifold.BodyA;

-                                RigidBody rbB = (RigidBody)manifold.BodyB;

-

-                                manifold.RefreshContactPoints(rbA.CenterOfMassTransform, rbB.CenterOfMassTransform);

-                        }

-

-                        int minReservation = manifolds.Count * 2;

-

-                        _tmpSolverBodyPool = new List<SolverBody>(minReservation);

-

-                        for (int i = 0; i < bodies.Count; i++)

-                        {

-                                RigidBody rb = RigidBody.Upcast(bodies[i]);

-                                if (rb != null && rb.IslandTag >= 0)

-                                {

-                                        BulletDebug.Assert(rb.CompanionID < 0);

-                                        int solverBodyId = _tmpSolverBodyPool.Count;

-                                        SolverBody solverBody;

-                                        InitSolverBody(out solverBody, rb);

-                                        _tmpSolverBodyPool.Add(solverBody);

-                                        rb.CompanionID = solverBodyId;

-                                }

-                        }

-

-                        _tmpSolverConstraintPool = new List<SolverConstraint>(minReservation);

-                        _tmpSolverFrictionConstraintPool = new List<SolverConstraint>(minReservation);

-

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

-                        {

-                                PersistentManifold manifold = manifolds[i];

-                                RigidBody rb0 = (RigidBody)manifold.BodyA;

-                                RigidBody rb1 = (RigidBody)manifold.BodyB;

-

-                                int solverBodyIdA = -1;

-                                int solverBodyIdB = -1;

-

-                                //if (i == 89)

-                                //    System.Diagnostics.Debugger.Break();

-

-                                if (manifold.ContactsCount != 0)

-                                {

-                                        if (rb0.IslandTag >= 0)

-                                        {

-                                                solverBodyIdA = rb0.CompanionID;

-                                        }

-                                        else

-                                        {

-                                                //create a static body

-                                                solverBodyIdA = _tmpSolverBodyPool.Count;

-                                                SolverBody solverBody;

-                                                InitSolverBody(out solverBody, rb0);

-                                                _tmpSolverBodyPool.Add(solverBody);

-                                        }

-

-                                        if (rb1.IslandTag >= 0)

-                                        {

-                                                solverBodyIdB = rb1.CompanionID;

-                                        }

-                                        else

-                                        {

-                                                //create a static body

-                                                solverBodyIdB = _tmpSolverBodyPool.Count;

-                                                SolverBody solverBody;

-                                                InitSolverBody(out solverBody, rb1);

-                                                _tmpSolverBodyPool.Add(solverBody);

-                                        }

-                                }

-

-                                if (solverBodyIdB == -1 || solverBodyIdA == -1)

-                                        System.Diagnostics.Debug.WriteLine(string.Format("We're in ass ! {0}", i));

-

-                                for (int j = 0; j < manifold.ContactsCount; j++)

-                                {

-                                        ManifoldPoint cp = manifold.GetContactPoint(j);

-

-                                        int frictionIndex = _tmpSolverConstraintPool.Count;

-

-                                        if (cp.Distance <= 0)

-                                        {

-

-                                                Vector3 pos1 = cp.PositionWorldOnA;

-                                                Vector3 pos2 = cp.PositionWorldOnB;

-

-                                                Vector3 rel_pos1 = pos1 - rb0.CenterOfMassPosition;

-                                                Vector3 rel_pos2 = pos2 - rb1.CenterOfMassPosition;

-

-                                                float relaxation = 1;

-                                                {

-                                                        SolverConstraint solverConstraint = new SolverConstraint();

-                                                        _tmpSolverConstraintPool.Add(solverConstraint);

-

-                                                        solverConstraint.SolverBodyIdA = solverBodyIdA;

-                                                        solverConstraint.SolverBodyIdB = solverBodyIdB;

-                                                        solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Contact;

-

-                                                        //can be optimized, the cross products are already calculated

-                                                        float denom0 = rb0.ComputeImpulseDenominator(pos1, cp.NormalWorldOnB);

-                                                        float denom1 = rb1.ComputeImpulseDenominator(pos2, cp.NormalWorldOnB);

-                                                        float denom = relaxation / (denom0 + denom1);

-                                                        solverConstraint.JacDiagABInv = denom;

-

-                                                        solverConstraint.ContactNormal = cp.NormalWorldOnB;

-                                                        solverConstraint.RelPosACrossNormal = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);

-                                                        solverConstraint.RelPosBCrossNormal = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);

-

-                                                        Vector3 vel1 = rb0.GetVelocityInLocalPoint(rel_pos1);

-                                                        Vector3 vel2 = rb1.GetVelocityInLocalPoint(rel_pos2);

-

-                                                        Vector3 vel = vel1 - vel2;

-                                                        float rel_vel;

-                                                        rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel);

-

-

-                                                        solverConstraint.Penetration = cp.Distance;//btScalar(infoGlobal.m_numIterations);

-                                                        solverConstraint.Friction = cp.CombinedFriction;

-                                                        float rest = RestitutionCurve(rel_vel, cp.CombinedRestitution);

-                                                        if (rest <= 0)

-                                                        {

-                                                                rest = 0;

-                                                        }

-

-                                                        float penVel = -solverConstraint.Penetration / infoGlobal.TimeStep;

-                                                        if (rest > penVel)

-                                                        {

-                                                                rest = 0;

-                                                        }

-                                                        solverConstraint.Restitution = rest;

-

-                                                        solverConstraint.Penetration *= -(infoGlobal.Erp / infoGlobal.TimeStep);

-

-                                                        solverConstraint.AppliedImpulse = 0f;

-                                                        solverConstraint.AppliedVelocityImpulse = 0f;

-

-#warning Check to see if we need Vector3.Transform

-                                                        Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);

-                                                        solverConstraint.AngularComponentA = Vector3.TransformNormal(torqueAxis0, rb0.InvInertiaTensorWorld);

-                                                        Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);

-                                                        solverConstraint.AngularComponentB = Vector3.TransformNormal(torqueAxis1, rb1.InvInertiaTensorWorld);

-                                                }

-                                                //create 2 '1d axis' constraints for 2 tangential friction directions

-

-                                                //re-calculate friction direction every frame, todo: check if this is really needed

-                                                Vector3 frictionTangential0a = new Vector3(),

-                                                                frictionTangential1b = new Vector3();

-

-                                                MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref frictionTangential0a, ref frictionTangential1b);

-                                                {

-                                                        SolverConstraint solverConstraint = new SolverConstraint();

-                                                        _tmpSolverFrictionConstraintPool.Add(solverConstraint);

-                                                        solverConstraint.ContactNormal = frictionTangential0a;

-

-                                                        solverConstraint.SolverBodyIdA = solverBodyIdA;

-                                                        solverConstraint.SolverBodyIdB = solverBodyIdB;

-                                                        solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction;

-                                                        solverConstraint.FrictionIndex = frictionIndex;

-

-                                                        solverConstraint.Friction = cp.CombinedFriction;

-

-                                                        solverConstraint.AppliedImpulse = 0;

-                                                        solverConstraint.AppliedVelocityImpulse = 0;

-

-                                                        float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal);

-                                                        float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal);

-                                                        float denom = relaxation / (denom0 + denom1);

-                                                        solverConstraint.JacDiagABInv = denom;

-

-                                                        {

-                                                                Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal);

-                                                                solverConstraint.RelPosACrossNormal = ftorqueAxis0;

-                                                                solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis0, rb0.InvInertiaTensorWorld);

-                                                        }

-                                                        {

-                                                                Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal);

-                                                                solverConstraint.RelPosBCrossNormal = ftorqueAxis0;

-                                                                solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis0, rb1.InvInertiaTensorWorld);

-                                                        }

-                                                }

-

-

-                                                {

-

-                                                        SolverConstraint solverConstraint = new SolverConstraint();

-                                                        _tmpSolverFrictionConstraintPool.Add(solverConstraint);

-                                                        solverConstraint.ContactNormal = frictionTangential1b;

-

-                                                        solverConstraint.SolverBodyIdA = solverBodyIdA;

-                                                        solverConstraint.SolverBodyIdB = solverBodyIdB;

-                                                        solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction;

-                                                        solverConstraint.FrictionIndex = frictionIndex;

-

-                                                        solverConstraint.Friction = cp.CombinedFriction;

-

-                                                        solverConstraint.AppliedImpulse = 0;

-                                                        solverConstraint.AppliedVelocityImpulse = 0;

-

-                                                        float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal);

-                                                        float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal);

-                                                        float denom = relaxation / (denom0 + denom1);

-                                                        solverConstraint.JacDiagABInv = denom;

-                                                        {

-                                                                Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal);

-                                                                solverConstraint.RelPosACrossNormal = ftorqueAxis1;

-                                                                solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis1, rb0.InvInertiaTensorWorld);

-                                                        }

-                                                        {

-                                                                Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal);

-                                                                solverConstraint.RelPosBCrossNormal = ftorqueAxis1;

-                                                                solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis1, rb1.InvInertiaTensorWorld);

-                                                        }

-                                                }

-                                        }

-                                }

-                        }

-

-                        ContactSolverInfo info = infoGlobal;

-                        {

-                                for (int j = 0; j < constraints.Count; j++)

-                                {

-                                        TypedConstraint constraint = constraints[j];

-                                        constraint.BuildJacobian();

-                                }

-                        }

-

-                        int numConstraintPool = _tmpSolverConstraintPool.Count;

-                        int numFrictionPool = _tmpSolverFrictionConstraintPool.Count;

-

-                        //todo: use stack allocator for such temporarily memory, same for solver bodies/constraints

-                        List<int> gOrderTmpConstraintPool = new List<int>(numConstraintPool);

-                        List<int> gOrderFrictionConstraintPool = new List<int>(numFrictionPool);

-                        {

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

-                                {

-                                        gOrderTmpConstraintPool.Add(i);

-                                }

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

-                                {

-                                        gOrderFrictionConstraintPool.Add(i);

-                                }

-                        }

-

-                        //should traverse the contacts random order...

-                        int iteration;

-                        {

-                                for (iteration = 0; iteration < info.IterationsCount; iteration++)

-                                {

-

-                                        int j;

-                                        if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None)

-                                        {

-                                                if ((iteration & 7) == 0)

-                                                {

-                                                        for (j = 0; j < numConstraintPool; ++j)

-                                                        {

-                                                                int tmp = gOrderTmpConstraintPool[j];

-                                                                int swapi = RandInt2(j + 1);

-                                                                gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi];

-                                                                gOrderTmpConstraintPool[swapi] = tmp;

-                                                        }

-

-                                                        for (j = 0; j < numFrictionPool; ++j)

-                                                        {

-                                                                int tmp = gOrderFrictionConstraintPool[j];

-                                                                int swapi = RandInt2(j + 1);

-                                                                gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi];

-                                                                gOrderFrictionConstraintPool[swapi] = tmp;

-                                                        }

-                                                }

-                                        }

-

-                                        for (j = 0; j < constraints.Count; j++)

-                                        {

-                                                TypedConstraint constraint = constraints[j];

-                                                //todo: use solver bodies, so we don't need to copy from/to btRigidBody

-

-                                                if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0))

-                                                {

-                                                        _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].WriteBackVelocity();

-                                                }

-                                                if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0))

-                                                {

-                                                        _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].WriteBackVelocity();

-                                                }

-

-                                                constraint.SolveConstraint(info.TimeStep);

-

-                                                if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0))

-                                                {

-                                                        _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].ReadVelocity();

-                                                }

-                                                if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0))

-                                                {

-                                                        _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].ReadVelocity();

-                                                }

-

-                                        }

-

-                                        {

-                                                int numPoolConstraints = _tmpSolverConstraintPool.Count;

-                                                for (j = 0; j < numPoolConstraints; j++)

-                                                {

-                                                        SolverConstraint solveManifold = _tmpSolverConstraintPool[gOrderTmpConstraintPool[j]];

-                                                        ResolveSingleCollisionCombinedCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA],

-                                                                _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info);

-                                                }

-                                        }

-

-                                        {

-                                                int numFrictionPoolConstraints = _tmpSolverFrictionConstraintPool.Count;

-                                                for (j = 0; j < numFrictionPoolConstraints; j++)

-                                                {

-                                                        SolverConstraint solveManifold = _tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]];

-                                                        float appliedNormalImpulse = _tmpSolverConstraintPool[solveManifold.FrictionIndex].AppliedImpulse;

-

-                                                        ResolveSingleFrictionCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA],

-                                                                _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info, appliedNormalImpulse);

-                                                }

-                                        }

-                                }

-                        }

-

-                        for (int i = 0; i < _tmpSolverBodyPool.Count; i++)

-                        {

-                                _tmpSolverBodyPool[i].WriteBackVelocity();

-                        }

-

-                        _tmpSolverBodyPool.Clear();

-                        _tmpSolverConstraintPool.Clear();

-                        _tmpSolverFrictionConstraintPool.Clear();

-

-                        return 0;

-                }

-

-                public virtual float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer)

-                {

-                        if ((_solverMode & SolverMode.CacheFriendly) != SolverMode.None)

-                        {

-                                return SolveGroupCacheFriendly(bodies, manifolds, numManifolds, constraints, infoGlobal, debugDrawer);

-                        }

-

-                        ContactSolverInfo info = infoGlobal;

-                        int totalPoints = 0;

-

-                        int numiter = infoGlobal.IterationsCount;

-

-                        for (int j = 0; j < manifolds.Count; j++)

-                        {

-                                PersistentManifold manifold = manifolds[j];

-                                PrepareConstraints(manifold, info);

-

-                                for (int p = 0; p < manifolds[j].ContactsCount; p++)

-                                {

-                                        _order[totalPoints].ManifoldIndex = j;

-                                        _order[totalPoints].PointIndex = p;

-                                        totalPoints++;

-                                }

-                        }

-

-                        for (int j = 0; j < constraints.Count; j++)

-                        {

-                                constraints[j].BuildJacobian();

-                        }

-

-                        //should traverse the contacts random order...

-                        int iteration;

-

-                        for (iteration = 0; iteration < numiter; iteration++)

-                        {

-                                int j;

-                                if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None)

-                                {

-                                        if ((iteration & 7) == 0)

-                                        {

-                                                for (j = 0; j < totalPoints; ++j)

-                                                {

-                                                        OrderIndex tmp = _order[j];

-                                                        int swapi = RandInt2(j + 1);

-                                                        _order[j] = _order[swapi];

-                                                        _order[swapi] = tmp;

-                                                }

-                                        }

-                                }

-

-                                for (j = 0; j < constraints.Count; j++)

-                                {

-                                        constraints[j].SolveConstraint(info.TimeStep);

-                                }

-

-                                for (j = 0; j < totalPoints; j++)

-                                {

-                                        PersistentManifold manifold = manifolds[_order[j].ManifoldIndex];

-                                        Solve((RigidBody)manifold.BodyA, (RigidBody)manifold.BodyB,

-                                                manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer);

-                                }

-

-                                for (j = 0; j < totalPoints; j++)

-                                {

-                                        PersistentManifold manifold = manifolds[_order[j].ManifoldIndex];

-                                        SolveFriction((RigidBody)manifold.BodyA,

-                                                (RigidBody)manifold.BodyB, manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer);

-                                }

-                        }

-

-                        return 0;

-                }

-

-                private void InitSolverBody(out SolverBody solverBody, RigidBody rigidBody)

-                {

-                        solverBody = new SolverBody();

-                        solverBody.AngularVelocity = rigidBody.AngularVelocity;

-                        solverBody.CenterOfMassPosition = rigidBody.CenterOfMassPosition;

-                        solverBody.Friction = rigidBody.Friction;

-                        solverBody.InvMass = rigidBody.InverseMass;

-                        solverBody.LinearVelocity = rigidBody.LinearVelocity;

-                        solverBody.OriginalBody = rigidBody;

-                        solverBody.AngularFactor = rigidBody.AngularFactor;

-                }

-

-                private long Rand2()

-                {

-                        _seed2 = (1664525L * _seed2 + 1013904223L) & 0xffffffff;

-                        return _seed2;

-                }

-

-                private int RandInt2(int n)

-                {

-                        // seems good; xor-fold and modulus

-                        long un = n;

-                        long r = Rand2();

-

-                        // note: probably more aggressive than it needs to be -- might be

-                        //       able to get away without one or two of the innermost branches.

-                        if (un <= 0x00010000L)

-                        {

-                                r ^= (r >> 16);

-                                if (un <= 0x00000100L)

-                                {

-                                        r ^= (r >> 8);

-                                        if (un <= 0x00000010L)

-                                        {

-                                                r ^= (r >> 4);

-                                                if (un <= 0x00000004L)

-                                                {

-                                                        r ^= (r >> 2);

-                                                        if (un <= 0x00000002L)

-                                                        {

-                                                                r ^= (r >> 1);

-                                                        }

-                                                }

-                                        }

-                                }

-                        }

-                        return (int)(r % un);

-                }

-

-                protected struct OrderIndex

-                {

-                        private int _manifoldIndex;

-                        private int _pointIndex;

-

-                        public int ManifoldIndex { get { return _manifoldIndex; } set { _manifoldIndex = value; } }

-                        public int PointIndex { get { return _pointIndex; } set { _pointIndex = 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.Dynamics
+{
+        [Flags]
+        public enum SolverMode
+        {
+                None = 0,
+                RandomizeOrder = 1,
+                FrictionSeperate = 2,
+                UseWarmstarting = 4,
+                CacheFriendly = 8,
+        }
+
+        public class SequentialImpulseConstraintSolver : IConstraintSolver
+        {
+                private static int _totalContactPoints = 0;
+
+                private SolverMode _solverMode;
+                private int _totalCpd = 0;
+                private ContactSolverFunc[,] _contactDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType];
+                private ContactSolverFunc[,] _frictionDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType];
+
+                private float _penetrationResolveFactor = 0.9f;
+                private List<SolverBody> _tmpSolverBodyPool = new List<SolverBody>();
+                private List<SolverConstraint> _tmpSolverConstraintPool = new List<SolverConstraint>();
+                private List<SolverConstraint> _tmpSolverFrictionConstraintPool = new List<SolverConstraint>();
+
+                private const int _sequentialImpulseMaxSolverPoints = 16384;
+                private static OrderIndex[] _order = new OrderIndex[SequentialImpulseMaxSolverPoints];
+                private static long _seed2 = 0;
+
+                public SequentialImpulseConstraintSolver()
+                {
+                        _solverMode = SolverMode.RandomizeOrder | SolverMode.CacheFriendly;
+                        PersistentManifold.ContactDestroyedCallback = MyContactDestroyedCallback;
+
+                        //initialize default friction/contact funcs
+                        int i, j;
+                        for (i = 0; i < (int)ContactSolverType.MaxContactSolverType; i++)
+                                for (j = 0; j < (int)ContactSolverType.MaxContactSolverType; j++)
+                                {
+
+                                        _contactDispatch[i, j] = ContactConstraint.ResolveSingleCollision;
+                                        _frictionDispatch[i, j] = ContactConstraint.ResolveSingleFriction;
+                                }
+                }
+
+                public SolverMode SolverMode { get { return _solverMode; } set { _solverMode = value; } }
+                public static int SequentialImpulseMaxSolverPoints { get { return _sequentialImpulseMaxSolverPoints; } }
+                protected static OrderIndex[] Order { get { return _order; } set { _order = value; } }
+                public static long RandSeed { get { return _seed2; } set { _seed2 = value; } }
+
+                ///<summary>
+                /// Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
+                /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
+                ///</summary>
+                public void SetContactSolverFunc(ContactSolverFunc func, int typeA, int typeB)
+                {
+                        _contactDispatch[typeA, typeB] = func;
+                }
+
+                /// <summary>
+                /// Advanced: Override the default friction solving function for contacts, for certain types of rigidbody
+                /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
+                ///</summary>
+                public void SetFrictionSolverFunc(ContactSolverFunc func, int typeA, int typeB)
+                {
+                        _frictionDispatch[typeA, typeB] = func;
+                }
+
+                protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
+                {
+                        float maxImpulse = 0;
+
+                        Vector3 color = new Vector3(0, 1, 0);
+                        if (cp.Distance <= 0)
+                        {
+                                if (iter == 0)
+                                        if(debugDraw != null)
+                                                debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);
+
+                                ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
+                                float impulse = cpd.ContactSolverFunc(
+                                        bodyA, bodyB,
+                                        cp,
+                                        info);
+
+                                if (maxImpulse < impulse)
+                                        maxImpulse = impulse;
+                        }
+                        return maxImpulse;
+                }
+
+                protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter)
+                {
+                        return Solve(bodyA, bodyB, cp, info, iter, null);
+                }
+
+                protected float SolveCombinedContactFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
+                {
+                        float maxImpulse = 0;
+
+                        Vector3 color = new Vector3(0, 1, 0);
+                        if (cp.Distance <= 0)
+                        {
+                                if (iter == 0)
+                                        if (debugDraw != null)
+                                                debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);
+
+                                float impulse = ContactConstraint.ResolveSingleCollisionCombined(
+                                        bodyA, bodyB,
+                                        cp,
+                                        info);
+
+                                if (maxImpulse < impulse)
+                                        maxImpulse = impulse;
+                        }
+                        return maxImpulse;
+                }
+
+                protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
+                {
+                        Vector3 color = new Vector3(0, 1, 0);
+
+                        if (cp.Distance <= 0)
+                        {
+
+                                ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
+                                cpd.FrictionSolverFunc(
+                                        bodyA, bodyB,
+                                        cp,
+                                        info);
+                        }
+                        return 0;
+                }
+
+                protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info)
+                {
+                        RigidBody body0 = manifold.BodyA as RigidBody;
+                        RigidBody body1 = manifold.BodyB as RigidBody;
+
+
+                        //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
+                        {
+                                manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform);
+
+                                int numpoints = manifold.ContactsCount;
+
+                                _totalContactPoints += numpoints;
+
+                                Vector3 color = new Vector3(0, 1, 0);
+                                for (int i = 0; i < numpoints; i++)
+                                {
+                                        ManifoldPoint cp = manifold.GetContactPoint(i);
+                                        if (cp.Distance <= 0)
+                                        {
+                                                Vector3 pos1 = cp.PositionWorldOnA;
+                                                Vector3 pos2 = cp.PositionWorldOnB;
+
+                                                Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition;
+                                                Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition;
+
+
+                                                //this jacobian entry is re-used for all iterations
+                                                JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform),
+                                                        MatrixOperations.Transpose(body1.CenterOfMassTransform),
+                                                        rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass,
+                                                        body1.InvInertiaDiagLocal, body1.InverseMass);
+
+                                                float jacDiagAB = jac.Diagonal;
+
+                                                ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
+                                                if (cpd != null)
+                                                {
+                                                        //might be invalid
+                                                        cpd.PersistentLifeTime++;
+                                                        if (cpd.PersistentLifeTime != cp.LifeTime)
+                                                        {
+                                                                //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
+                                                                cpd = new ConstraintPersistentData();
+                                                                cpd.PersistentLifeTime = cp.LifeTime;
+
+                                                        }
+                                                }
+                                                else
+                                                {
+
+                                                        cpd = new ConstraintPersistentData();
+                                                        _totalCpd++;
+                                                        //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
+                                                        cp.UserPersistentData = cpd;
+                                                        cpd.PersistentLifeTime = cp.LifeTime;
+                                                        //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
+
+                                                }
+                                                if (cpd == null)
+                                                        throw new BulletException();
+
+                                                cpd.JacDiagABInv = 1f / jacDiagAB;
+
+                                                //Dependent on Rigidbody A and B types, fetch the contact/friction response func
+                                                //perhaps do a similar thing for friction/restutution combiner funcs...
+
+                                                cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType];
+                                                cpd.ContactSolverFunc = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType];
+
+                                                Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1);
+                                                Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2);
+                                                Vector3 vel = vel1 - vel2;
+                                                float rel_vel;
+                                                rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel);
+
+                                                float combinedRestitution = cp.CombinedRestitution;
+
+                                                cpd.Penetration = cp.Distance;
+                                                cpd.Friction = cp.CombinedFriction;
+                                                cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution);
+                                                if (cpd.Restitution < 0f)
+                                                {
+                                                        cpd.Restitution = 0.0f;
+
+                                                };
+
+                                                //restitution and penetration work in same direction so
+                                                //rel_vel 
+
+                                                float penVel = -cpd.Penetration / info.TimeStep;
+
+                                                if (cpd.Restitution > penVel)
+                                                {
+                                                        cpd.Penetration = 0;
+                                                }
+
+
+                                                float relaxation = info.Damping;
+                                                if ((_solverMode & SolverMode.UseWarmstarting) != 0)
+                                                {
+                                                        cpd.AppliedImpulse *= relaxation;
+                                                }
+                                                else
+                                                {
+                                                        cpd.AppliedImpulse = 0f;
+                                                }
+
+                                                //for friction
+                                                cpd.PreviousAppliedImpulse = cpd.AppliedImpulse;
+
+                                                //re-calculate friction direction every frame, todo: check if this is really needed
+                                                Vector3 fwta = cpd.FrictionWorldTangentialA;
+                                                Vector3 fwtb = cpd.FrictionWorldTangentialB;
+                                                MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb);
+                                                cpd.FrictionWorldTangentialA = fwta;
+                                                cpd.FrictionWorldTangentialB = fwtb;
+
+                                                cpd.AccumulatedTangentImpulseA = 0;
+                                                cpd.AccumulatedTangentImpulseB = 0;
+                                                float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA);
+                                                float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA);
+                                                float denom = relaxation / (denom0 + denom1);
+                                                cpd.JacDiagABInvTangentA = denom;
+
+
+                                                denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB);
+                                                denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB);
+                                                denom = relaxation / (denom0 + denom1);
+                                                cpd.JacDiagABInvTangentB = denom;
+
+                                                Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse;
+
+                                                {
+                                                        Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
+                                                        cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld);
+                                                        Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);
+                                                        cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld);
+                                                }
+                                                {
+                                                        Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA);
+                                                        cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld);
+                                                }
+                                                {
+                                                        Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB);
+                                                        cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld);
+                                                }
+                                                {
+                                                        Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA);
+                                                        cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld);
+                                                }
+                                                {
+                                                        Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB);
+                                                        cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld);
+                                                }
+
+
+                                                //apply previous frames impulse on both bodies
+                                                body0.ApplyImpulse(totalImpulse, rel_pos1);
+                                                body1.ApplyImpulse(-totalImpulse, rel_pos2);
+                                        }
+                                }
+                        }
+                }
+
+                private bool MyContactDestroyedCallback(object userPersistentData)
+                {
+                        if (userPersistentData == null)
+                                throw new BulletException();
+                        ConstraintPersistentData cpd = userPersistentData as ConstraintPersistentData;
+                        _totalCpd--;
+                        return true;
+                }
+
+                private float RestitutionCurve(float relVel, float restitution)
+                {
+                        float rest = restitution * -relVel;
+                        return rest;
+                }
+
+                //velocity + friction
+                //response  between two dynamic objects with friction
+                public virtual float ResolveSingleCollisionCombinedCacheFriendly(
+                        SolverBody bodyA,
+                        SolverBody bodyB,
+                        SolverConstraint contactConstraint,
+                        ContactSolverInfo solverInfo)
+                {
+                        float normalImpulse = 0;
+
+                        if (contactConstraint.Penetration < 0)
+                                return 0;
+
+                        float relVel;
+                        float velADotn = Vector3.Dot(contactConstraint.ContactNormal,bodyA.LinearVelocity)
+                                                + Vector3.Dot(contactConstraint.RelPosACrossNormal,bodyA.AngularVelocity);
+                        float velBDotn = Vector3.Dot(contactConstraint.ContactNormal,bodyB.LinearVelocity)
+                                                + Vector3.Dot(contactConstraint.RelPosBCrossNormal,bodyB.AngularVelocity);
+
+                        relVel = velADotn - velBDotn;
+
+                        float positionalError = contactConstraint.Penetration;
+                        float velocityError = contactConstraint.Restitution - relVel;// * damping;
+
+                        float penetrationImpulse = positionalError * contactConstraint.JacDiagABInv;
+                        float velocityImpulse = velocityError * contactConstraint.JacDiagABInv;
+                        normalImpulse = penetrationImpulse + velocityImpulse;
+
+                        // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
+                        float oldNormalImpulse = contactConstraint.AppliedImpulse;
+                        float sum = oldNormalImpulse + normalImpulse;
+                        contactConstraint.AppliedImpulse = 0 > sum ? 0 : sum;
+
+                        float oldVelocityImpulse = contactConstraint.AppliedVelocityImpulse;
+                        float velocitySum = oldVelocityImpulse + velocityImpulse;
+                        contactConstraint.AppliedVelocityImpulse = 0 > velocitySum ? 0 : velocitySum;
+
+                        normalImpulse = contactConstraint.AppliedImpulse - oldNormalImpulse;
+
+                        if (bodyA.InvMass != 0)
+                        {
+                                bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass,
+                                        contactConstraint.AngularComponentA, normalImpulse);
+                        }
+                        if (bodyB.InvMass != 0)
+                        {
+                                bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass,
+                                        contactConstraint.AngularComponentB, -normalImpulse);
+                        }
+
+                        return normalImpulse;
+                }
+
+                public virtual float ResolveSingleFrictionCacheFriendly(
+                        SolverBody bodyA,
+                        SolverBody bodyB,
+                        SolverConstraint contactConstraint,
+                        ContactSolverInfo solverInfo,
+                        float appliedNormalImpulse)
+                {
+                        float combinedFriction = contactConstraint.Friction;
+                        float limit = appliedNormalImpulse * combinedFriction;
+
+                        if (appliedNormalImpulse > 0)
+                        //friction
+                        {
+                                float j1;
+                                {
+                                        float relVel;
+                                        float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity)
+                                                                + Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity);
+                                        float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity)
+                                                + Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity);
+                                        relVel = velADotn - velBDotn;
+
+                                        // calculate j that moves us to zero relative velocity
+                                        j1 = -relVel * contactConstraint.JacDiagABInv;
+                                        float oldTangentImpulse = contactConstraint.AppliedImpulse;
+                                        contactConstraint.AppliedImpulse = oldTangentImpulse + j1;
+
+                                        float test = contactConstraint.AppliedImpulse;
+                                        MathHelper.SetMin(ref test, limit);
+                                        MathHelper.SetMax(ref test, -limit);
+                                        contactConstraint.AppliedImpulse = test;
+
+                                        j1 = contactConstraint.AppliedImpulse - oldTangentImpulse;
+                                }
+
+                                if (bodyA.InvMass != 0)
+                                {
+                                        bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, j1);
+                                }
+                                if (bodyB.InvMass != 0)
+                                {
+                                        bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -j1);
+                                }
+                        }
+                        return 0;
+                }
+
+                public virtual float SolveGroupCacheFriendly(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer)
+                {
+                        if (constraints.Count + numManifolds == 0)
+                        {
+                                return 0;
+                        }
+
+                        for (int i = 0; i < numManifolds; i++)
+                        {
+                                PersistentManifold manifold = manifolds[i];
+                                RigidBody rbA = (RigidBody)manifold.BodyA;
+                                RigidBody rbB = (RigidBody)manifold.BodyB;
+
+                                manifold.RefreshContactPoints(rbA.CenterOfMassTransform, rbB.CenterOfMassTransform);
+                        }
+
+                        int minReservation = manifolds.Count * 2;
+
+                        _tmpSolverBodyPool = new List<SolverBody>(minReservation);
+
+                        for (int i = 0; i < bodies.Count; i++)
+                        {
+                                RigidBody rb = RigidBody.Upcast(bodies[i]);
+                                if (rb != null && rb.IslandTag >= 0)
+                                {
+                                        BulletDebug.Assert(rb.CompanionID < 0);
+                                        int solverBodyId = _tmpSolverBodyPool.Count;
+                                        SolverBody solverBody;
+                                        InitSolverBody(out solverBody, rb);
+                                        _tmpSolverBodyPool.Add(solverBody);
+                                        rb.CompanionID = solverBodyId;
+                                }
+                        }
+
+                        _tmpSolverConstraintPool = new List<SolverConstraint>(minReservation);
+                        _tmpSolverFrictionConstraintPool = new List<SolverConstraint>(minReservation);
+
+                        for (int i = 0; i < numManifolds; i++)
+                        {
+                                PersistentManifold manifold = manifolds[i];
+                                RigidBody rb0 = (RigidBody)manifold.BodyA;
+                                RigidBody rb1 = (RigidBody)manifold.BodyB;
+
+                                int solverBodyIdA = -1;
+                                int solverBodyIdB = -1;
+
+                                //if (i == 89)
+                                //    System.Diagnostics.Debugger.Break();
+
+                                if (manifold.ContactsCount != 0)
+                                {
+                                        if (rb0.IslandTag >= 0)
+                                        {
+                                                solverBodyIdA = rb0.CompanionID;
+                                        }
+                                        else
+                                        {
+                                                //create a static body
+                                                solverBodyIdA = _tmpSolverBodyPool.Count;
+                                                SolverBody solverBody;
+                                                InitSolverBody(out solverBody, rb0);
+                                                _tmpSolverBodyPool.Add(solverBody);
+                                        }
+
+                                        if (rb1.IslandTag >= 0)
+                                        {
+                                                solverBodyIdB = rb1.CompanionID;
+                                        }
+                                        else
+                                        {
+                                                //create a static body
+                                                solverBodyIdB = _tmpSolverBodyPool.Count;
+                                                SolverBody solverBody;
+                                                InitSolverBody(out solverBody, rb1);
+                                                _tmpSolverBodyPool.Add(solverBody);
+                                        }
+                                }
+
+                                if (solverBodyIdB == -1 || solverBodyIdA == -1)
+                                        System.Diagnostics.Debug.WriteLine(string.Format("We're in ass ! {0}", i));
+
+                                for (int j = 0; j < manifold.ContactsCount; j++)
+                                {
+                                        ManifoldPoint cp = manifold.GetContactPoint(j);
+
+                                        int frictionIndex = _tmpSolverConstraintPool.Count;
+
+                                        if (cp.Distance <= 0)
+                                        {
+
+                                                Vector3 pos1 = cp.PositionWorldOnA;
+                                                Vector3 pos2 = cp.PositionWorldOnB;
+
+                                                Vector3 rel_pos1 = pos1 - rb0.CenterOfMassPosition;
+                                                Vector3 rel_pos2 = pos2 - rb1.CenterOfMassPosition;
+
+                                                float relaxation = 1;
+                                                {
+                                                        SolverConstraint solverConstraint = new SolverConstraint();
+                                                        _tmpSolverConstraintPool.Add(solverConstraint);
+
+                                                        solverConstraint.SolverBodyIdA = solverBodyIdA;
+                                                        solverConstraint.SolverBodyIdB = solverBodyIdB;
+                                                        solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Contact;
+
+                                                        //can be optimized, the cross products are already calculated
+                                                        float denom0 = rb0.ComputeImpulseDenominator(pos1, cp.NormalWorldOnB);
+                                                        float denom1 = rb1.ComputeImpulseDenominator(pos2, cp.NormalWorldOnB);
+                                                        float denom = relaxation / (denom0 + denom1);
+                                                        solverConstraint.JacDiagABInv = denom;
+
+                                                        solverConstraint.ContactNormal = cp.NormalWorldOnB;
+                                                        solverConstraint.RelPosACrossNormal = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
+                                                        solverConstraint.RelPosBCrossNormal = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);
+
+                                                        Vector3 vel1 = rb0.GetVelocityInLocalPoint(rel_pos1);
+                                                        Vector3 vel2 = rb1.GetVelocityInLocalPoint(rel_pos2);
+
+                                                        Vector3 vel = vel1 - vel2;
+                                                        float rel_vel;
+                                                        rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel);
+
+
+                                                        solverConstraint.Penetration = cp.Distance;//btScalar(infoGlobal.m_numIterations);
+                                                        solverConstraint.Friction = cp.CombinedFriction;
+                                                        float rest = RestitutionCurve(rel_vel, cp.CombinedRestitution);
+                                                        if (rest <= 0)
+                                                        {
+                                                                rest = 0;
+                                                        }
+
+                                                        float penVel = -solverConstraint.Penetration / infoGlobal.TimeStep;
+                                                        if (rest > penVel)
+                                                        {
+                                                                rest = 0;
+                                                        }
+                                                        solverConstraint.Restitution = rest;
+
+                                                        solverConstraint.Penetration *= -(infoGlobal.Erp / infoGlobal.TimeStep);
+
+                                                        solverConstraint.AppliedImpulse = 0f;
+                                                        solverConstraint.AppliedVelocityImpulse = 0f;
+
+#warning Check to see if we need Vector3.Transform
+                                                        Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
+                                                        solverConstraint.AngularComponentA = Vector3.TransformNormal(torqueAxis0, rb0.InvInertiaTensorWorld);
+                                                        Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);
+                                                        solverConstraint.AngularComponentB = Vector3.TransformNormal(torqueAxis1, rb1.InvInertiaTensorWorld);
+                                                }
+                                                //create 2 '1d axis' constraints for 2 tangential friction directions
+
+                                                //re-calculate friction direction every frame, todo: check if this is really needed
+                                                Vector3 frictionTangential0a = new Vector3(),
+                                                                frictionTangential1b = new Vector3();
+
+                                                MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref frictionTangential0a, ref frictionTangential1b);
+                                                {
+                                                        SolverConstraint solverConstraint = new SolverConstraint();
+                                                        _tmpSolverFrictionConstraintPool.Add(solverConstraint);
+                                                        solverConstraint.ContactNormal = frictionTangential0a;
+
+                                                        solverConstraint.SolverBodyIdA = solverBodyIdA;
+                                                        solverConstraint.SolverBodyIdB = solverBodyIdB;
+                                                        solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction;
+                                                        solverConstraint.FrictionIndex = frictionIndex;
+
+                                                        solverConstraint.Friction = cp.CombinedFriction;
+
+                                                        solverConstraint.AppliedImpulse = 0;
+                                                        solverConstraint.AppliedVelocityImpulse = 0;
+
+                                                        float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal);
+                                                        float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal);
+                                                        float denom = relaxation / (denom0 + denom1);
+                                                        solverConstraint.JacDiagABInv = denom;
+
+                                                        {
+                                                                Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal);
+                                                                solverConstraint.RelPosACrossNormal = ftorqueAxis0;
+                                                                solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis0, rb0.InvInertiaTensorWorld);
+                                                        }
+                                                        {
+                                                                Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal);
+                                                                solverConstraint.RelPosBCrossNormal = ftorqueAxis0;
+                                                                solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis0, rb1.InvInertiaTensorWorld);
+                                                        }
+                                                }
+
+
+                                                {
+
+                                                        SolverConstraint solverConstraint = new SolverConstraint();
+                                                        _tmpSolverFrictionConstraintPool.Add(solverConstraint);
+                                                        solverConstraint.ContactNormal = frictionTangential1b;
+
+                                                        solverConstraint.SolverBodyIdA = solverBodyIdA;
+                                                        solverConstraint.SolverBodyIdB = solverBodyIdB;
+                                                        solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction;
+                                                        solverConstraint.FrictionIndex = frictionIndex;
+
+                                                        solverConstraint.Friction = cp.CombinedFriction;
+
+                                                        solverConstraint.AppliedImpulse = 0;
+                                                        solverConstraint.AppliedVelocityImpulse = 0;
+
+                                                        float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal);
+                                                        float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal);
+                                                        float denom = relaxation / (denom0 + denom1);
+                                                        solverConstraint.JacDiagABInv = denom;
+                                                        {
+                                                                Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal);
+                                                                solverConstraint.RelPosACrossNormal = ftorqueAxis1;
+                                                                solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis1, rb0.InvInertiaTensorWorld);
+                                                        }
+                                                        {
+                                                                Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal);
+                                                                solverConstraint.RelPosBCrossNormal = ftorqueAxis1;
+                                                                solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis1, rb1.InvInertiaTensorWorld);
+                                                        }
+                                                }
+                                        }
+                                }
+                        }
+
+                        ContactSolverInfo info = infoGlobal;
+                        {
+                                for (int j = 0; j < constraints.Count; j++)
+                                {
+                                        TypedConstraint constraint = constraints[j];
+                                        constraint.BuildJacobian();
+                                }
+                        }
+
+                        int numConstraintPool = _tmpSolverConstraintPool.Count;
+                        int numFrictionPool = _tmpSolverFrictionConstraintPool.Count;
+
+                        //todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
+                        List<int> gOrderTmpConstraintPool = new List<int>(numConstraintPool);
+                        List<int> gOrderFrictionConstraintPool = new List<int>(numFrictionPool);
+                        {
+                                for (int i = 0; i < numConstraintPool; i++)
+                                {
+                                        gOrderTmpConstraintPool.Add(i);
+                                }
+                                for (int i = 0; i < numFrictionPool; i++)
+                                {
+                                        gOrderFrictionConstraintPool.Add(i);
+                                }
+                        }
+
+                        //should traverse the contacts random order...
+                        int iteration;
+                        {
+                                for (iteration = 0; iteration < info.IterationsCount; iteration++)
+                                {
+
+                                        int j;
+                                        if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None)
+                                        {
+                                                if ((iteration & 7) == 0)
+                                                {
+                                                        for (j = 0; j < numConstraintPool; ++j)
+                                                        {
+                                                                int tmp = gOrderTmpConstraintPool[j];
+                                                                int swapi = RandInt2(j + 1);
+                                                                gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi];
+                                                                gOrderTmpConstraintPool[swapi] = tmp;
+                                                        }
+
+                                                        for (j = 0; j < numFrictionPool; ++j)
+                                                        {
+                                                                int tmp = gOrderFrictionConstraintPool[j];
+                                                                int swapi = RandInt2(j + 1);
+                                                                gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi];
+                                                                gOrderFrictionConstraintPool[swapi] = tmp;
+                                                        }
+                                                }
+                                        }
+
+                                        for (j = 0; j < constraints.Count; j++)
+                                        {
+                                                TypedConstraint constraint = constraints[j];
+                                                //todo: use solver bodies, so we don't need to copy from/to btRigidBody
+
+                                                if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0))
+                                                {
+                                                        _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].WriteBackVelocity();
+                                                }
+                                                if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0))
+                                                {
+                                                        _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].WriteBackVelocity();
+                                                }
+
+                                                constraint.SolveConstraint(info.TimeStep);
+
+                                                if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0))
+                                                {
+                                                        _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].ReadVelocity();
+                                                }
+                                                if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0))
+                                                {
+                                                        _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].ReadVelocity();
+                                                }
+
+                                        }
+
+                                        {
+                                                int numPoolConstraints = _tmpSolverConstraintPool.Count;
+                                                for (j = 0; j < numPoolConstraints; j++)
+                                                {
+                                                        SolverConstraint solveManifold = _tmpSolverConstraintPool[gOrderTmpConstraintPool[j]];
+                                                        ResolveSingleCollisionCombinedCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA],
+                                                                _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info);
+                                                }
+                                        }
+
+                                        {
+                                                int numFrictionPoolConstraints = _tmpSolverFrictionConstraintPool.Count;
+                                                for (j = 0; j < numFrictionPoolConstraints; j++)
+                                                {
+                                                        SolverConstraint solveManifold = _tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]];
+                                                        float appliedNormalImpulse = _tmpSolverConstraintPool[solveManifold.FrictionIndex].AppliedImpulse;
+
+                                                        ResolveSingleFrictionCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA],
+                                                                _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info, appliedNormalImpulse);
+                                                }
+                                        }
+                                }
+                        }
+
+                        for (int i = 0; i < _tmpSolverBodyPool.Count; i++)
+                        {
+                                _tmpSolverBodyPool[i].WriteBackVelocity();
+                        }
+
+                        _tmpSolverBodyPool.Clear();
+                        _tmpSolverConstraintPool.Clear();
+                        _tmpSolverFrictionConstraintPool.Clear();
+
+                        return 0;
+                }
+
+                public virtual float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer)
+                {
+                        if ((_solverMode & SolverMode.CacheFriendly) != SolverMode.None)
+                        {
+                                return SolveGroupCacheFriendly(bodies, manifolds, numManifolds, constraints, infoGlobal, debugDrawer);
+                        }
+
+                        ContactSolverInfo info = infoGlobal;
+                        int totalPoints = 0;
+
+                        int numiter = infoGlobal.IterationsCount;
+
+                        for (int j = 0; j < manifolds.Count; j++)
+                        {
+                                PersistentManifold manifold = manifolds[j];
+                                PrepareConstraints(manifold, info);
+
+                                for (int p = 0; p < manifolds[j].ContactsCount; p++)
+                                {
+                                        _order[totalPoints].ManifoldIndex = j;
+                                        _order[totalPoints].PointIndex = p;
+                                        totalPoints++;
+                                }
+                        }
+
+                        for (int j = 0; j < constraints.Count; j++)
+                        {
+                                constraints[j].BuildJacobian();
+                        }
+
+                        //should traverse the contacts random order...
+                        int iteration;
+
+                        for (iteration = 0; iteration < numiter; iteration++)
+                        {
+                                int j;
+                                if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None)
+                                {
+                                        if ((iteration & 7) == 0)
+                                        {
+                                                for (j = 0; j < totalPoints; ++j)
+                                                {
+                                                        OrderIndex tmp = _order[j];
+                                                        int swapi = RandInt2(j + 1);
+                                                        _order[j] = _order[swapi];
+                                                        _order[swapi] = tmp;
+                                                }
+                                        }
+                                }
+
+                                for (j = 0; j < constraints.Count; j++)
+                                {
+                                        constraints[j].SolveConstraint(info.TimeStep);
+                                }
+
+                                for (j = 0; j < totalPoints; j++)
+                                {
+                                        PersistentManifold manifold = manifolds[_order[j].ManifoldIndex];
+                                        Solve((RigidBody)manifold.BodyA, (RigidBody)manifold.BodyB,
+                                                manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer);
+                                }
+
+                                for (j = 0; j < totalPoints; j++)
+                                {
+                                        PersistentManifold manifold = manifolds[_order[j].ManifoldIndex];
+                                        SolveFriction((RigidBody)manifold.BodyA,
+                                                (RigidBody)manifold.BodyB, manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer);
+                                }
+                        }
+
+                        return 0;
+                }
+
+                private void InitSolverBody(out SolverBody solverBody, RigidBody rigidBody)
+                {
+                        solverBody = new SolverBody();
+                        solverBody.AngularVelocity = rigidBody.AngularVelocity;
+                        solverBody.CenterOfMassPosition = rigidBody.CenterOfMassPosition;
+                        solverBody.Friction = rigidBody.Friction;
+                        solverBody.InvMass = rigidBody.InverseMass;
+                        solverBody.LinearVelocity = rigidBody.LinearVelocity;
+                        solverBody.OriginalBody = rigidBody;
+                        solverBody.AngularFactor = rigidBody.AngularFactor;
+                }
+
+                private long Rand2()
+                {
+                        _seed2 = (1664525L * _seed2 + 1013904223L) & 0xffffffff;
+                        return _seed2;
+                }
+
+                private int RandInt2(int n)
+                {
+                        // seems good; xor-fold and modulus
+                        long un = n;
+                        long r = Rand2();
+
+                        // note: probably more aggressive than it needs to be -- might be
+                        //       able to get away without one or two of the innermost branches.
+                        if (un <= 0x00010000L)
+                        {
+                                r ^= (r >> 16);
+                                if (un <= 0x00000100L)
+                                {
+                                        r ^= (r >> 8);
+                                        if (un <= 0x00000010L)
+                                        {
+                                                r ^= (r >> 4);
+                                                if (un <= 0x00000004L)
+                                                {
+                                                        r ^= (r >> 2);
+                                                        if (un <= 0x00000002L)
+                                                        {
+                                                                r ^= (r >> 1);
+                                                        }
+                                                }
+                                        }
+                                }
+                        }
+                        return (int)(r % un);
+                }
+
+                protected struct OrderIndex
+                {
+                        private int _manifoldIndex;
+                        private int _pointIndex;
+
+                        public int ManifoldIndex { get { return _manifoldIndex; } set { _manifoldIndex = value; } }
+                        public int PointIndex { get { return _pointIndex; } set { _pointIndex = value; } }
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs
index 6ced783..9d4060d 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs
@@ -1,188 +1,188 @@
-/*

-  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.Dynamics

-{

-        /// <summary>

-        /// constraint class used for lateral tyre friction

-        /// </summary>

-        public class Solve2LinearConstraint

-        {

-                private float _tau;

-                private float _damping;

-

-                public Solve2LinearConstraint(float tau, float damping)

-                {

-                        _tau = tau;

-                        _damping = damping;

-                }

-

-                // solve unilateral constraint (equality, direct method)

-                public void ResolveUnilateralPairConstraint(

-                                                        RigidBody body1, RigidBody body2,

-                                                        Matrix world2A,

-                                                        Matrix world2B,

-                                                        Vector3 invInertiaADiag,

-                                                        float invMassA,

-                                                        Vector3 linvelA, Vector3 angvelA,

-                                                        Vector3 rel_posA1,

-                                                        Vector3 invInertiaBDiag,

-                                                        float invMassB,

-                                                        Vector3 linvelB, Vector3 angvelB,

-                                                        Vector3 rel_posA2,

-                                                        float depthA, Vector3 normalA,

-                                                        Vector3 rel_posB1, Vector3 rel_posB2,

-                                                        float depthB, Vector3 normalB,

-                                                        out float imp0, out float imp1)

-                {

-                        imp0 = 0;

-                        imp1 = 0;

-

-                        float len = Math.Abs(normalA.Length()) - 1f;

-                        if (Math.Abs(len) >= float.Epsilon)

-                                return;

-

-                        BulletDebug.Assert(len < float.Epsilon);

-

-                        //this jacobian entry could be re-used for all iterations

-                        JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,

-                                invInertiaBDiag, invMassB);

-                        JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,

-                                invInertiaBDiag, invMassB);

-

-                        float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1));

-                        float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1));

-

-                        //      btScalar penetrationImpulse = (depth*contactTau*timeCorrection)  * massTerm;//jacDiagABInv

-                        float massTerm = 1f / (invMassA + invMassB);

-

-                        // calculate rhs (or error) terms

-                        float dv0 = depthA * _tau * massTerm - vel0 * _damping;

-                        float dv1 = depthB * _tau * massTerm - vel1 * _damping;

-

-                        float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);

-                        float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag);

-

-                        imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet;

-                        imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet;

-                }

-

-                // solving 2x2 lcp problem (inequality, direct solution )

-                public void ResolveBilateralPairConstraint(

-                                                        RigidBody body1, RigidBody body2,

-                                                        Matrix world2A, Matrix world2B,

-                                                        Vector3 invInertiaADiag,

-                                                        float invMassA,

-                                                        Vector3 linvelA, Vector3 angvelA,

-                                                        Vector3 rel_posA1,

-                                                        Vector3 invInertiaBDiag,

-                                                        float invMassB,

-                                                        Vector3 linvelB, Vector3 angvelB,

-                                                        Vector3 rel_posA2,

-                                                        float depthA, Vector3 normalA,

-                                                        Vector3 rel_posB1, Vector3 rel_posB2,

-                                                        float depthB, Vector3 normalB,

-                                                        out float imp0, out float imp1)

-                {

-                        imp0 = 0f;

-                        imp1 = 0f;

-

-                        float len = Math.Abs(normalA.Length()) - 1f;

-                        if (Math.Abs(len) >= float.Epsilon)

-                                return;

-

-                        BulletDebug.Assert(len < float.Epsilon);

-

-                        JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,

-                                invInertiaBDiag, invMassB);

-                        JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,

-                                invInertiaBDiag, invMassB);

-

-                        float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1));

-                        float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1));

-

-                        // calculate rhs (or error) terms

-                        float dv0 = depthA * _tau - vel0 * _damping;

-                        float dv1 = depthB * _tau - vel1 * _damping;

-

-                        float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);

-                        float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag);

-

-                        imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet;

-                        imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet;

-

-                        if (imp0 > 0.0f)

-                        {

-                                if (imp1 <= 0.0f)

-                                {

-                                        imp1 = 0f;

-

-                                        // now imp0>0 imp1<0

-                                        imp0 = dv0 / jacA.Diagonal;

-                                        if (imp0 < 0.0f)

-                                                imp0 = 0f;

-                                }

-                        }

-                        else

-                        {

-                                imp0 = 0f;

-

-                                imp1 = dv1 / jacB.Diagonal;

-                                if (imp1 <= 0.0f)

-                                {

-                                        imp1 = 0f;

-                                        // now imp0>0 imp1<0

-                                        imp0 = dv0 / jacA.Diagonal;

-                                        if (imp0 > 0.0f)

-                                        {

-                                        }

-                                        else

-                                        {

-                                                imp0 = 0f;

-                                        }

-                                }

-                        }

-                }

-

-                //public void ResolveAngularConstraint(

-                //                Matrix invInertiaAWS,

-                //                float invMassA,

-                //                Vector3 linvelA, Vector3 angvelA,

-                //                Vector3 rel_posA1,

-                //                Matrix invInertiaBWS,

-                //                float invMassB,

-                //                Vector3 linvelB, Vector3 angvelB,

-                //                Vector3 rel_posA2,

-                //                float depthA, Vector3 normalA,

-                //                Vector3 rel_posB1, Vector3 rel_posB2,

-                //                float depthB, Vector3 normalB,

-                //                out float imp0, out float imp1)

-                //{

-                //    imp0 = 0;

-                //    imp1 = 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.Dynamics
+{
+        /// <summary>
+        /// constraint class used for lateral tyre friction
+        /// </summary>
+        public class Solve2LinearConstraint
+        {
+                private float _tau;
+                private float _damping;
+
+                public Solve2LinearConstraint(float tau, float damping)
+                {
+                        _tau = tau;
+                        _damping = damping;
+                }
+
+                // solve unilateral constraint (equality, direct method)
+                public void ResolveUnilateralPairConstraint(
+                                                        RigidBody body1, RigidBody body2,
+                                                        Matrix world2A,
+                                                        Matrix world2B,
+                                                        Vector3 invInertiaADiag,
+                                                        float invMassA,
+                                                        Vector3 linvelA, Vector3 angvelA,
+                                                        Vector3 rel_posA1,
+                                                        Vector3 invInertiaBDiag,
+                                                        float invMassB,
+                                                        Vector3 linvelB, Vector3 angvelB,
+                                                        Vector3 rel_posA2,
+                                                        float depthA, Vector3 normalA,
+                                                        Vector3 rel_posB1, Vector3 rel_posB2,
+                                                        float depthB, Vector3 normalB,
+                                                        out float imp0, out float imp1)
+                {
+                        imp0 = 0;
+                        imp1 = 0;
+
+                        float len = Math.Abs(normalA.Length()) - 1f;
+                        if (Math.Abs(len) >= float.Epsilon)
+                                return;
+
+                        BulletDebug.Assert(len < float.Epsilon);
+
+                        //this jacobian entry could be re-used for all iterations
+                        JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
+                                invInertiaBDiag, invMassB);
+                        JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
+                                invInertiaBDiag, invMassB);
+
+                        float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1));
+                        float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1));
+
+                        //      btScalar penetrationImpulse = (depth*contactTau*timeCorrection)  * massTerm;//jacDiagABInv
+                        float massTerm = 1f / (invMassA + invMassB);
+
+                        // calculate rhs (or error) terms
+                        float dv0 = depthA * _tau * massTerm - vel0 * _damping;
+                        float dv1 = depthB * _tau * massTerm - vel1 * _damping;
+
+                        float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);
+                        float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag);
+
+                        imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet;
+                        imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet;
+                }
+
+                // solving 2x2 lcp problem (inequality, direct solution )
+                public void ResolveBilateralPairConstraint(
+                                                        RigidBody body1, RigidBody body2,
+                                                        Matrix world2A, Matrix world2B,
+                                                        Vector3 invInertiaADiag,
+                                                        float invMassA,
+                                                        Vector3 linvelA, Vector3 angvelA,
+                                                        Vector3 rel_posA1,
+                                                        Vector3 invInertiaBDiag,
+                                                        float invMassB,
+                                                        Vector3 linvelB, Vector3 angvelB,
+                                                        Vector3 rel_posA2,
+                                                        float depthA, Vector3 normalA,
+                                                        Vector3 rel_posB1, Vector3 rel_posB2,
+                                                        float depthB, Vector3 normalB,
+                                                        out float imp0, out float imp1)
+                {
+                        imp0 = 0f;
+                        imp1 = 0f;
+
+                        float len = Math.Abs(normalA.Length()) - 1f;
+                        if (Math.Abs(len) >= float.Epsilon)
+                                return;
+
+                        BulletDebug.Assert(len < float.Epsilon);
+
+                        JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
+                                invInertiaBDiag, invMassB);
+                        JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
+                                invInertiaBDiag, invMassB);
+
+                        float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1));
+                        float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1));
+
+                        // calculate rhs (or error) terms
+                        float dv0 = depthA * _tau - vel0 * _damping;
+                        float dv1 = depthB * _tau - vel1 * _damping;
+
+                        float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);
+                        float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag);
+
+                        imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet;
+                        imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet;
+
+                        if (imp0 > 0.0f)
+                        {
+                                if (imp1 <= 0.0f)
+                                {
+                                        imp1 = 0f;
+
+                                        // now imp0>0 imp1<0
+                                        imp0 = dv0 / jacA.Diagonal;
+                                        if (imp0 < 0.0f)
+                                                imp0 = 0f;
+                                }
+                        }
+                        else
+                        {
+                                imp0 = 0f;
+
+                                imp1 = dv1 / jacB.Diagonal;
+                                if (imp1 <= 0.0f)
+                                {
+                                        imp1 = 0f;
+                                        // now imp0>0 imp1<0
+                                        imp0 = dv0 / jacA.Diagonal;
+                                        if (imp0 > 0.0f)
+                                        {
+                                        }
+                                        else
+                                        {
+                                                imp0 = 0f;
+                                        }
+                                }
+                        }
+                }
+
+                //public void ResolveAngularConstraint(
+                //                Matrix invInertiaAWS,
+                //                float invMassA,
+                //                Vector3 linvelA, Vector3 angvelA,
+                //                Vector3 rel_posA1,
+                //                Matrix invInertiaBWS,
+                //                float invMassB,
+                //                Vector3 linvelB, Vector3 angvelB,
+                //                Vector3 rel_posA2,
+                //                float depthA, Vector3 normalA,
+                //                Vector3 rel_posB1, Vector3 rel_posB2,
+                //                float depthB, Vector3 normalB,
+                //                out float imp0, out float imp1)
+                //{
+                //    imp0 = 0;
+                //    imp1 = 0;
+                //}
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs
index 09b73cc..6996272 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.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;

-using System.Runtime.InteropServices;

-

-namespace XnaDevRu.BulletX.Dynamics

-{

-        public class SolverBody

-        {

-                private Vector3 _centerOfMassPosition = new Vector3();

-                private Vector3 _linearVelocity = new Vector3();

-                private Vector3 _angularVelocity = new Vector3();

-                private RigidBody _originalBody = null;

-                private float _invMass;

-                private float _friction;

-                private float _angularFactor;

-

-                public Vector3 CenterOfMassPosition { get { return _centerOfMassPosition; } set { _centerOfMassPosition = value; } }

-                public Vector3 LinearVelocity { get { return _linearVelocity; } set { _linearVelocity = value; } }

-                public Vector3 AngularVelocity { get { return _angularVelocity; } set { _angularVelocity = value; } }

-                public RigidBody OriginalBody { get { return _originalBody; } set { _originalBody = value; } }

-                public float InvMass { get { return _invMass; } set { _invMass = value; } }

-                public float Friction { get { return _friction; } set { _friction = value; } }

-                public float AngularFactor { get { return _angularFactor; } set { _angularFactor = value; } }

-

-                public void GetVelocityInLocalPoint(Vector3 relPos, out Vector3 velocity)

-                {

-                        velocity = _linearVelocity + Vector3.Cross(_angularVelocity, relPos);

-                }

-

-                public void WriteBackVelocity()

-                {

-                        if (_invMass != 0)

-                        {

-                                _originalBody.LinearVelocity = _linearVelocity;

-                                _originalBody.AngularVelocity = _angularVelocity;

-                        }

-                }

-

-                public void ReadVelocity()

-                {

-                        if (_invMass != 0)

-                        {

-                                _linearVelocity = _originalBody.LinearVelocity;

-                                _angularVelocity = _originalBody.AngularVelocity;

-                        }

-                }

-

-                //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position

-                internal void ApplyImpulse(Vector3 linearComponent, Vector3 angularComponent, float impulseMagnitude)

-                {

-                        _linearVelocity += linearComponent * impulseMagnitude;

-                        _angularVelocity += angularComponent * impulseMagnitude * _angularFactor;

-                }

-        }

-}

+/*
+  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;
+using System.Runtime.InteropServices;
+
+namespace XnaDevRu.BulletX.Dynamics
+{
+        public class SolverBody
+        {
+                private Vector3 _centerOfMassPosition = new Vector3();
+                private Vector3 _linearVelocity = new Vector3();
+                private Vector3 _angularVelocity = new Vector3();
+                private RigidBody _originalBody = null;
+                private float _invMass;
+                private float _friction;
+                private float _angularFactor;
+
+                public Vector3 CenterOfMassPosition { get { return _centerOfMassPosition; } set { _centerOfMassPosition = value; } }
+                public Vector3 LinearVelocity { get { return _linearVelocity; } set { _linearVelocity = value; } }
+                public Vector3 AngularVelocity { get { return _angularVelocity; } set { _angularVelocity = value; } }
+                public RigidBody OriginalBody { get { return _originalBody; } set { _originalBody = value; } }
+                public float InvMass { get { return _invMass; } set { _invMass = value; } }
+                public float Friction { get { return _friction; } set { _friction = value; } }
+                public float AngularFactor { get { return _angularFactor; } set { _angularFactor = value; } }
+
+                public void GetVelocityInLocalPoint(Vector3 relPos, out Vector3 velocity)
+                {
+                        velocity = _linearVelocity + Vector3.Cross(_angularVelocity, relPos);
+                }
+
+                public void WriteBackVelocity()
+                {
+                        if (_invMass != 0)
+                        {
+                                _originalBody.LinearVelocity = _linearVelocity;
+                                _originalBody.AngularVelocity = _angularVelocity;
+                        }
+                }
+
+                public void ReadVelocity()
+                {
+                        if (_invMass != 0)
+                        {
+                                _linearVelocity = _originalBody.LinearVelocity;
+                                _angularVelocity = _originalBody.AngularVelocity;
+                        }
+                }
+
+                //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
+                internal void ApplyImpulse(Vector3 linearComponent, Vector3 angularComponent, float impulseMagnitude)
+                {
+                        _linearVelocity += linearComponent * impulseMagnitude;
+                        _angularVelocity += angularComponent * impulseMagnitude * _angularFactor;
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs
index 3e3a64a..4fd0361 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs
@@ -1,77 +1,77 @@
-/*

-  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;

-using System.Runtime.InteropServices;

-

-namespace XnaDevRu.BulletX.Dynamics

-{

-        //1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.

-        public class SolverConstraint

-        {

-                private Vector3 _relpos1CrossNormal = new Vector3();

-                private Vector3 _relpos2CrossNormal = new Vector3();

-                private Vector3 _contactNormal = new Vector3();

-                private Vector3 _angularComponentA = new Vector3();

-                private Vector3 _angularComponentB = new Vector3();

-

-                private float _appliedVelocityImpulse;

-                private int _solverBodyIdA;

-                int _solverBodyIdB;

-                private float _friction;

-                private float _restitution;

-                private float _jacDiagABInv;

-                private float _penetration;

-                private float _appliedImpulse;

-

-                private SolverConstraintType _constraintType = SolverConstraintType.Contact;

-                private int _frictionIndex;

-                private int[] _unusedPadding = new int[2];

-

-                public Vector3 RelPosACrossNormal { get { return _relpos1CrossNormal; } set { _relpos1CrossNormal = value; } }

-                public Vector3 RelPosBCrossNormal { get { return _relpos2CrossNormal; } set { _relpos2CrossNormal = value; } }

-                public Vector3 ContactNormal { get { return _contactNormal; } set { _contactNormal = value; } }

-                public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } }

-                public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } }

-

-                public float AppliedVelocityImpulse { get { return _appliedVelocityImpulse; } set { _appliedVelocityImpulse = value; } }

-                public int SolverBodyIdA { get { return _solverBodyIdA; } set { _solverBodyIdA = value; } }

-                public int SolverBodyIdB { get { return _solverBodyIdB; } set { _solverBodyIdB = value; } }

-                public float Friction { get { return _friction; } set { _friction = value; } }

-                public float Restitution { get { return _restitution; } set { _restitution = value; } }

-                public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } }

-                public float Penetration { get { return _penetration; } set { _penetration = value; } }

-                public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } }

-

-                public SolverConstraintType ConstraintType { get { return _constraintType; } set { _constraintType = value; } }

-                public int FrictionIndex { get { return _frictionIndex; } set { _frictionIndex = value; } }

-                public int[] UnusedPadding { get { return _unusedPadding; } set { _unusedPadding = value; } }

-

-                public enum SolverConstraintType

-                {

-                        Contact = 0,

-                        Friction,

-                }

-        }

-}

+/*
+  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;
+using System.Runtime.InteropServices;
+
+namespace XnaDevRu.BulletX.Dynamics
+{
+        //1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
+        public class SolverConstraint
+        {
+                private Vector3 _relpos1CrossNormal = new Vector3();
+                private Vector3 _relpos2CrossNormal = new Vector3();
+                private Vector3 _contactNormal = new Vector3();
+                private Vector3 _angularComponentA = new Vector3();
+                private Vector3 _angularComponentB = new Vector3();
+
+                private float _appliedVelocityImpulse;
+                private int _solverBodyIdA;
+                int _solverBodyIdB;
+                private float _friction;
+                private float _restitution;
+                private float _jacDiagABInv;
+                private float _penetration;
+                private float _appliedImpulse;
+
+                private SolverConstraintType _constraintType = SolverConstraintType.Contact;
+                private int _frictionIndex;
+                private int[] _unusedPadding = new int[2];
+
+                public Vector3 RelPosACrossNormal { get { return _relpos1CrossNormal; } set { _relpos1CrossNormal = value; } }
+                public Vector3 RelPosBCrossNormal { get { return _relpos2CrossNormal; } set { _relpos2CrossNormal = value; } }
+                public Vector3 ContactNormal { get { return _contactNormal; } set { _contactNormal = value; } }
+                public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } }
+                public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } }
+
+                public float AppliedVelocityImpulse { get { return _appliedVelocityImpulse; } set { _appliedVelocityImpulse = value; } }
+                public int SolverBodyIdA { get { return _solverBodyIdA; } set { _solverBodyIdA = value; } }
+                public int SolverBodyIdB { get { return _solverBodyIdB; } set { _solverBodyIdB = value; } }
+                public float Friction { get { return _friction; } set { _friction = value; } }
+                public float Restitution { get { return _restitution; } set { _restitution = value; } }
+                public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } }
+                public float Penetration { get { return _penetration; } set { _penetration = value; } }
+                public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } }
+
+                public SolverConstraintType ConstraintType { get { return _constraintType; } set { _constraintType = value; } }
+                public int FrictionIndex { get { return _frictionIndex; } set { _frictionIndex = value; } }
+                public int[] UnusedPadding { get { return _unusedPadding; } set { _unusedPadding = value; } }
+
+                public enum SolverConstraintType
+                {
+                        Contact = 0,
+                        Friction,
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs
index abd10f1..8131463 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs
@@ -1,88 +1,88 @@
-/*

-  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.Dynamics

-{

-        public abstract class TypedConstraint

-        {

-                private static RigidBody _fixed = new RigidBody(0, null, null, new Vector3(), 0, 0, 0.5f, 0);

-                private int _userConstraintType;

-                private int _userConstraintId;

-

-                private RigidBody _rbA;

-                private RigidBody _rbB;

-                private float _appliedImpulse;

-

-                public TypedConstraint()

-                        : this(_fixed, _fixed) { }

-

-                public TypedConstraint(RigidBody rbA)

-                        : this(rbA, _fixed) { }

-

-                public TypedConstraint(RigidBody rbA, RigidBody rbB)

-                {

-                        _userConstraintType = -1;

-                        _userConstraintId = -1;

-                        _rbA = rbA;

-                        _rbB = rbB;

-                        _appliedImpulse = 0;

-

-                        _fixed.SetMassProps(0, new Vector3());

-                }

-

-                public virtual RigidBody RigidBodyA { get { return _rbA; } protected set { _rbA = value; } }

-                public virtual RigidBody RigidBodyB { get { return _rbB; } protected set { _rbB = value; } }

-

-                public float AppliedImpulse { get { return _appliedImpulse; } protected set { _appliedImpulse = value; } }

-                public int UserConstraintId { get { return _userConstraintId; } set { _userConstraintId = value; } }

-                public int UserConstraintType { get { return _userConstraintType; } set { _userConstraintType = value; } }

-

-                public abstract void BuildJacobian();

-                public abstract void SolveConstraint(float timeStep);

-

-                public static int SortConstraintOnIslandPredicate(TypedConstraint left, TypedConstraint right)

-                {

-                        int rightIslandID, leftIslandID;

-                        rightIslandID = GetConstraintIslandId(right);

-                        leftIslandID = GetConstraintIslandId(left);

-                        if (leftIslandID < rightIslandID)

-                                return -1;

-                        else

-                                return 1;

-                        return 0;

-                }

-

-                internal static int GetConstraintIslandId(TypedConstraint lhs)

-                {

-                        int islandId;

-

-                        CollisionObject colObjA = lhs.RigidBodyA;

-                        CollisionObject colObjB = lhs.RigidBodyB;

-                        islandId = colObjA.IslandTag >= 0 ? colObjA.IslandTag : colObjB.IslandTag;

-                        return islandId;

-                }

-        }

-}

+/*
+  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.Dynamics
+{
+        public abstract class TypedConstraint
+        {
+                private static RigidBody _fixed = new RigidBody(0, null, null, new Vector3(), 0, 0, 0.5f, 0);
+                private int _userConstraintType;
+                private int _userConstraintId;
+
+                private RigidBody _rbA;
+                private RigidBody _rbB;
+                private float _appliedImpulse;
+
+                public TypedConstraint()
+                        : this(_fixed, _fixed) { }
+
+                public TypedConstraint(RigidBody rbA)
+                        : this(rbA, _fixed) { }
+
+                public TypedConstraint(RigidBody rbA, RigidBody rbB)
+                {
+                        _userConstraintType = -1;
+                        _userConstraintId = -1;
+                        _rbA = rbA;
+                        _rbB = rbB;
+                        _appliedImpulse = 0;
+
+                        _fixed.SetMassProps(0, new Vector3());
+                }
+
+                public virtual RigidBody RigidBodyA { get { return _rbA; } protected set { _rbA = value; } }
+                public virtual RigidBody RigidBodyB { get { return _rbB; } protected set { _rbB = value; } }
+
+                public float AppliedImpulse { get { return _appliedImpulse; } protected set { _appliedImpulse = value; } }
+                public int UserConstraintId { get { return _userConstraintId; } set { _userConstraintId = value; } }
+                public int UserConstraintType { get { return _userConstraintType; } set { _userConstraintType = value; } }
+
+                public abstract void BuildJacobian();
+                public abstract void SolveConstraint(float timeStep);
+
+                public static int SortConstraintOnIslandPredicate(TypedConstraint left, TypedConstraint right)
+                {
+                        int rightIslandID, leftIslandID;
+                        rightIslandID = GetConstraintIslandId(right);
+                        leftIslandID = GetConstraintIslandId(left);
+                        if (leftIslandID < rightIslandID)
+                                return -1;
+                        else
+                                return 1;
+                        return 0;
+                }
+
+                internal static int GetConstraintIslandId(TypedConstraint lhs)
+                {
+                        int islandId;
+
+                        CollisionObject colObjA = lhs.RigidBodyA;
+                        CollisionObject colObjB = lhs.RigidBodyB;
+                        islandId = colObjA.IslandTag >= 0 ? colObjA.IslandTag : colObjB.IslandTag;
+                        return islandId;
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs
index acceb98..3702c2a 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs
@@ -1,790 +1,790 @@
-/*

-  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.Dynamics

-{

-        /// <summary>

-        /// DiscreteDynamicsWorld provides discrete rigid body simulation

-        /// those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController

-        /// </summary>

-        public class DiscreteDynamicsWorld : DynamicsWorld

-        {

-                private static bool _reportMe = true;

-

-                private IConstraintSolver _constraintSolver;

-                private SimulationIslandManager _islandManager;

-                private List<TypedConstraint> _constraints = new List<TypedConstraint>();

-                private IDebugDraw _debugDrawer;

-                private ContactSolverInfo _solverInfo = new ContactSolverInfo();

-                private Vector3 _gravity;

-                //for variable timesteps

-                private float _localTime;

-                //for variable timesteps

-                private bool _ownsIslandManager;

-                private bool _ownsConstraintSolver;

-                private List<RaycastVehicle> _vehicles = new List<RaycastVehicle>();

-                private int _profileTimings;

-

-                public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache)

-                        : this(dispatcher, pairCache, null) { }

-

-                //this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those

-                public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver)

-                        : base(dispatcher, pairCache)

-                {

-                        _constraintSolver = constraintSolver != null ? constraintSolver : new SequentialImpulseConstraintSolver();

-                        _debugDrawer = null;

-                        _gravity = new Vector3(0, -10, 0);

-                        _localTime = 1f / 60f;

-                        _profileTimings = 0;

-                        _islandManager = new SimulationIslandManager();

-                        _ownsIslandManager = true;

-                        _ownsConstraintSolver = constraintSolver == null;

-                }

-

-                public ContactSolverInfo SolverInfo { get { return _solverInfo; } }

-                public SimulationIslandManager SimulationIslandManager { get { return _islandManager; } }

-                public CollisionWorld CollisionWorld { get { return this; } }

-

-                public override IDebugDraw DebugDrawer

-                {

-                        get

-                        {

-                                return _debugDrawer;

-                        }

-                        set

-                        {

-                                _debugDrawer = value;

-                        }

-                }

-

-                public override Vector3 Gravity

-                {

-                        set

-                        {

-                                _gravity = value;

-                                for (int i = 0; i < CollisionObjects.Count; i++)

-                                {

-                                        CollisionObject colObj = CollisionObjects[i];

-                                        RigidBody body = RigidBody.Upcast(colObj);

-                                        if (body != null)

-                                        {

-                                                body.Gravity = value;

-                                        }

-                                }

-                        }

-                }

-

-                public override IConstraintSolver ConstraintSolver

-                {

-                        set

-                        {

-                                _ownsConstraintSolver = false;

-                                _constraintSolver = value;

-                        }

-                }

-

-                public override int ConstraintsCount

-                {

-                        get

-                        {

-                                return _constraints.Count;

-                        }

-                }

-

-                //if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's

-                public override void StepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep)

-                {

-                        int numSimulationSubSteps = 0;

-

-                        if (maxSubSteps != 0)

-                        {

-                                //fixed timestep with interpolation

-                                _localTime += timeStep;

-                                if (_localTime >= fixedTimeStep)

-                                {

-                                        numSimulationSubSteps = (int)(_localTime / fixedTimeStep);

-                                        _localTime -= numSimulationSubSteps * fixedTimeStep;

-                                }

-                        }

-                        else

-                        {

-                                //variable timestep

-                                fixedTimeStep = timeStep;

-                                _localTime = timeStep;

-                                if (Math.Abs(timeStep) < float.Epsilon)

-                                {

-                                        numSimulationSubSteps = 0;

-                                        maxSubSteps = 0;

-                                }

-                                else

-                                {

-                                        numSimulationSubSteps = 1;

-                                        maxSubSteps = 1;

-                                }

-                        }

-

-                        //process some debugging flags

-                        if (DebugDrawer != null)

-                        {

-                                RigidBody.DisableDeactivation = (DebugDrawer.DebugMode & DebugDrawModes.NoDeactivation) != 0;

-                        }

-                        if (numSimulationSubSteps != 0)

-                        {

-

-                                SaveKinematicState(fixedTimeStep);

-

-                                //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt

-                                int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps;

-

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

-                                {

-                                        InternalSingleStepSimulation(fixedTimeStep);

-                                        SynchronizeMotionStates();

-                                }

-

-                        }

-

-                        SynchronizeMotionStates();

-

-                        //return numSimulationSubSteps;

-                }

-

-                public void StepSimulation(float timeStep, int maxSubSteps)

-                {

-                        StepSimulation(timeStep, maxSubSteps, 1f / 60f);

-                }

-

-                public override void UpdateAabbs()

-                {

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

-                        for (int i = 0; i < CollisionObjects.Count; i++)

-                        {

-                                CollisionObject colObj = CollisionObjects[i];

-                                RigidBody body = RigidBody.Upcast(colObj);

-

-                                if (body != null)

-                                {

-                                        //      if (body->IsActive() && (!body->IsStatic()))

-                                        {

-                                                Vector3 minAabb, maxAabb;

-                                                colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb);

-                                                OverlappingPairCache bp = BroadphasePairCache;

-

-                                                //moving objects should be moderately sized, probably something wrong if not

-                                                if (colObj.IsStaticObject || ((maxAabb - minAabb).LengthSquared() < 1e12f))

-                                                {

-                                                        bp.SetAabb(body.Broadphase, minAabb, maxAabb);

-                                                }

-                                                else

-                                                {

-                                                        //something went wrong, investigate

-                                                        //this assert is unwanted in 3D modelers (danger of loosing work)

-                                                        BulletDebug.Assert(false);

-                                                        body.ActivationState = ActivationState.DisableSimulation;

-

-                                                        if (_reportMe)

-                                                        {

-                                                                _reportMe = false;

-                                                                Console.WriteLine("Overflow in AABB, object removed from simulation \n");

-                                                                Console.WriteLine("If you can reproduce this, please email bugs@continuousphysics.com\n");

-                                                                Console.WriteLine("Please include above information, your Platform, version of OS.\n");

-                                                                Console.WriteLine("Thanks.\n");

-                                                        }

-                                                }

-                                                if (_debugDrawer != null && (_debugDrawer.DebugMode & DebugDrawModes.DrawAabb) != 0)

-                                                        DrawAabb(_debugDrawer, minAabb, maxAabb, colorvec);

-                                        }

-                                }

-                        }

-                }

-

-                public override void AddConstraint(TypedConstraint constraint)

-                {

-                        _constraints.Add(constraint);

-                }

-

-                public override void RemoveConstraint(TypedConstraint constraint)

-                {

-                        _constraints.Remove(constraint);

-                }

-

-                public void AddVehicle(RaycastVehicle vehicle)

-                {

-                        _vehicles.Add(vehicle);

-                }

-

-                public void RemoveVehicle(RaycastVehicle vehicle)

-                {

-                        _vehicles.Remove(vehicle);

-                }

-

-                public override void AddRigidBody(RigidBody body)

-                {

-                        if (!body.IsStaticOrKinematicObject)

-                        {

-                                body.Gravity = _gravity;

-                        }

-

-                        if (body.CollisionShape != null)

-                        {

-                                bool isDynamic = !(body.IsStaticObject || body.IsKinematicObject);

-                                BroadphaseProxy.CollisionFilterGroups collisionFilterGroup = isDynamic ? BroadphaseProxy.CollisionFilterGroups.Default : BroadphaseProxy.CollisionFilterGroups.Static;

-                                BroadphaseProxy.CollisionFilterGroups collisionFilterMask = isDynamic ? BroadphaseProxy.CollisionFilterGroups.All : (BroadphaseProxy.CollisionFilterGroups.All ^ BroadphaseProxy.CollisionFilterGroups.Static);

-

-                                AddCollisionObject(body, collisionFilterGroup, collisionFilterMask);

-                        }

-                }

-

-                public override void RemoveRigidBody(RigidBody body)

-                {

-                        RemoveCollisionObject(body);

-                }

-

-                public void DebugDrawObject(Matrix worldTransform, CollisionShape shape, Vector3 color)

-                {

-                        if (shape.ShapeType == BroadphaseNativeTypes.Compound)

-                        {

-                                CompoundShape compoundShape = shape as CompoundShape;

-                                for (int i = compoundShape.ChildShapeCount - 1; i >= 0; i--)

-                                {

-                                        Matrix childTrans = compoundShape.GetChildTransform(i);

-                                        CollisionShape colShape = compoundShape.GetChildShape(i);

-                                        DebugDrawObject(worldTransform * childTrans, colShape, color);

-                                }

-

-                        }

-                        else

-                        {

-                                switch (shape.ShapeType)

-                                {

-

-                                        case BroadphaseNativeTypes.Sphere:

-                                                {

-                                                        SphereShape sphereShape = shape as SphereShape;

-                                                        float radius = sphereShape.Margin;//radius doesn't include the margin, so draw with margin

-                                                        Vector3 start = worldTransform.Translation;

-                                                        DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(radius, 0, 0), worldTransform), color);

-                                                        DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, radius, 0), worldTransform), color);

-                                                        DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, 0, radius), worldTransform), color);

-                                                        //drawSphere                                    

-                                                        break;

-                                                }

-                                        case BroadphaseNativeTypes.MultiSphere:

-                                        case BroadphaseNativeTypes.Cone:

-                                                {

-                                                        ConeShape coneShape = shape as ConeShape;

-                                                        float radius = coneShape.Radius;//+coneShape->getMargin();

-                                                        float height = coneShape.Height;//+coneShape->getMargin();

-                                                        Vector3 start = worldTransform.Translation;

-                                                        DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(radius, 0f, -0.5f * height), worldTransform), color);

-                                                        DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(-radius, 0f, -0.5f * height), worldTransform), color);

-                                                        DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, radius, -0.5f * height), worldTransform), color);

-                                                        DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, -radius, -0.5f * height), worldTransform), color);

-                                                        break;

-                                                }

-                                        case BroadphaseNativeTypes.Cylinder:

-                                                {

-                                                        CylinderShape cylinder = shape as CylinderShape;

-                                                        int upAxis = cylinder.UpAxis;

-                                                        float radius = cylinder.Radius;

-                                                        float halfHeight = MathHelper.GetElement(cylinder.HalfExtents, upAxis);

-                                                        Vector3 start = worldTransform.Translation;

-                                                        Vector3 offsetHeight = new Vector3();

-                                                        MathHelper.SetElement(ref offsetHeight, upAxis, halfHeight);

-                                                        Vector3 offsetRadius = new Vector3();

-                                                        MathHelper.SetElement(ref offsetRadius, (upAxis + 1) % 3,  radius);

-                                                        DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight + offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight + offsetRadius, worldTransform), color);

-                                                        DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight - offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight - offsetRadius, worldTransform), color);

-                                                        break;

-                                                } 

-                                        default:

-                                                {

-                                                        if (shape.ShapeType == BroadphaseNativeTypes.TriangleMesh)

-                                                        {

-                                                                TriangleMeshShape concaveMesh = shape as TriangleMeshShape;

-                                                                //btVector3 aabbMax(1e30f,1e30f,1e30f);

-                                                                //btVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f);

-

-                                                                //todo pass camera, for some culling

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

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

-

-                                                                DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color);

-                                                                concaveMesh.ProcessAllTriangles(drawCallback, aabbMin, aabbMax);

-                                                        }

-

-                                                        if (shape.ShapeType == BroadphaseNativeTypes.ConvexTriangleMesh)

-                                                        {

-                                                                ConvexTriangleMeshShape convexMesh = shape as ConvexTriangleMeshShape;

-                                                                //todo: pass camera for some culling                    

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

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

-                                                                //DebugDrawcallback drawCallback;

-                                                                DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color);

-                                                                convexMesh.getStridingMesh().InternalProcessAllTriangles(drawCallback, aabbMin, aabbMax);

-                                                        }

-

-                                                        // for polyhedral shapes

-                                                        if (shape.IsPolyhedral)

-                                                        {

-                                                                PolyhedralConvexShape polyshape = shape as PolyhedralConvexShape;

-

-                                                                for (int i = 0; i < polyshape.EdgeCount; i++)

-                                                                {

-                                                                        Vector3 a, b;

-                                                                        polyshape.GetEdge(i, out a, out b);

-                                                                        a = Vector3.TransformNormal(a, worldTransform);

-                                                                        b = Vector3.TransformNormal(b, worldTransform);

-                                                                        DebugDrawer.DrawLine(a, b, color);

-                                                                }

-                                                        }

-                                                        break;

-                                                }

-                                }

-                        }

-                }

-

-                public override TypedConstraint GetConstraint(int index)

-                {

-                        return _constraints[index];

-                }

-

-                public static void DrawAabb(IDebugDraw debugDrawer, Vector3 from, Vector3 to, Vector3 color)

-                {

-                        Vector3 halfExtents = (to - from) * 0.5f;

-                        Vector3 center = (to + from) * 0.5f;

-

-                        Vector3 edgecoord = new Vector3(1f, 1f, 1f), pa, pb;

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

-                        {

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

-                                {

-                                        pa = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y,

-                                                edgecoord.Z * halfExtents.Z);

-                                        pa += center;

-

-                                        int othercoord = j % 3;

-                                        MathHelper.SetElement(ref edgecoord, othercoord, MathHelper.GetElement(edgecoord, othercoord) * -1f);

-                                        pb = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y,

-                                                edgecoord.Z * halfExtents.Z);

-                                        pb += center;

-

-                                        debugDrawer.DrawLine(pa, pb, color);

-                                }

-                                edgecoord = new Vector3(-1f, -1f, -1f);

-                                if (i < 3)

-                                        MathHelper.SetElement(ref edgecoord, i, MathHelper.GetElement(edgecoord, i) * -1f);

-                        }

-                }

-

-                protected void PredictUnconstraintMotion(float timeStep)

-                {

-                        for (int i = 0; i < CollisionObjects.Count; i++)

-                        {

-                                CollisionObject colObj = CollisionObjects[i];

-                                RigidBody body = RigidBody.Upcast(colObj);

-                                if (body != null)

-                                {

-                                        if (!body.IsStaticOrKinematicObject)

-                                        {

-                                                if (body.IsActive)

-                                                {

-                                                        body.ApplyForces(timeStep);

-                                                        body.IntegrateVelocities(timeStep);

-                                                        Matrix temp = body.InterpolationWorldTransform;

-                                                        body.PredictIntegratedTransform(timeStep, ref temp);

-                                                        body.InterpolationWorldTransform = temp;

-                                                }

-                                        }

-                                }

-                        }

-                }

-

-                protected void IntegrateTransforms(float timeStep)

-                {

-                        Matrix predictedTrans = new Matrix();

-                        for (int i = 0; i < CollisionObjects.Count; i++)

-                        {

-                                CollisionObject colObj = CollisionObjects[i];

-                                RigidBody body = RigidBody.Upcast(colObj);

-                                if (body != null)

-                                {

-                                        if (body.IsActive && (!body.IsStaticOrKinematicObject))

-                                        {

-                                                body.PredictIntegratedTransform(timeStep, ref predictedTrans);

-                                                body.ProceedToTransform(predictedTrans);

-                                        }

-                                }

-                        }

-                }

-

-                protected void CalculateSimulationIslands()

-                {

-                        SimulationIslandManager.UpdateActivationState(this, Dispatcher);

-

-                        for (int i = 0; i < _constraints.Count; i++)

-                        {

-                                TypedConstraint constraint = _constraints[i];

-

-                                RigidBody colObj0 = constraint.RigidBodyA;

-                                RigidBody colObj1 = constraint.RigidBodyB;

-

-                                if (((colObj0 != null) && (colObj0.MergesSimulationIslands)) &&

-                                        ((colObj1 != null) && (colObj1.MergesSimulationIslands)))

-                                {

-                                        if (colObj0.IsActive || colObj1.IsActive)

-                                        {

-

-                                                SimulationIslandManager.UnionFind.Unite((colObj0).IslandTag,

-                                                        (colObj1).IslandTag);

-                                        }

-                                }

-                        }

-

-                        //Store the island id in each body

-                        SimulationIslandManager.StoreIslandActivationState(this);

-                }

-

-                //protected void SolveNonContactConstraints(ContactSolverInfo solverInfo)

-                //{

-                //    //constraint preparation: building jacobians

-                //    for (int i = 0; i < _constraints.Count; i++)

-                //    {

-                //        TypedConstraint constraint = _constraints[i];

-                //        constraint.BuildJacobian();

-                //    }

-

-                //    //solve the regular non-contact constraints (point 2 point, hinge, generic d6)

-                //    for (int g = 0; g < solverInfo.IterationsCount; g++)

-                //    {

-                //        for (int i = 0; i < _constraints.Count; i++)

-                //        {

-                //            TypedConstraint constraint = _constraints[i];

-                //            constraint.SolveConstraint(solverInfo.TimeStep);

-                //        }

-                //    }

-                //}

-

-                //protected void SolveContactConstraints(ContactSolverInfo solverInfo)

-                //{

-                //    InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, _debugDrawer);

-

-                //    // solve all the contact points and contact friction

-                //    _islandManager.BuildAndProcessIslands(Dispatcher, CollisionObjects, solverCallback);

-                //}

-

-                protected void SolveConstraints(ContactSolverInfo solverInfo)

-                {

-                        //sorted version of all btTypedConstraint, based on islandId

-                        List<TypedConstraint> sortedConstraints = new List<TypedConstraint>(ConstraintsCount);

-

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

-                        {

-                                sortedConstraints.Add(_constraints[i]);

-                        }

-

-                        sortedConstraints.Sort(new Comparison<TypedConstraint>(TypedConstraint.SortConstraintOnIslandPredicate));

-                        List<TypedConstraint> constraintsPtr = ConstraintsCount != 0 ? sortedConstraints : new List<TypedConstraint>();

-

-                        InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, constraintsPtr,  _debugDrawer);

-

-                        // solve all the constraints for this island

-                        _islandManager.BuildAndProcessIslands(CollisionWorld.Dispatcher, CollisionWorld.CollisionObjects, solverCallback);

-                }

-

-                protected void UpdateActivationState(float timeStep)

-                {

-                        for (int i = 0; i < CollisionObjects.Count; i++)

-                        {

-                                CollisionObject colObj = CollisionObjects[i];

-                                RigidBody body = RigidBody.Upcast(colObj);

-                                if (body != null)

-                                {

-                                        body.UpdateDeactivation(timeStep);

-

-                                        if (body.WantsSleeping())

-                                        {

-                                                if (body.IsStaticOrKinematicObject)

-                                                {

-                                                        body.ActivationState = ActivationState.IslandSleeping;

-                                                }

-                                                else

-                                                {

-                                                        if (body.ActivationState == ActivationState.Active)

-                                                                body.ActivationState = ActivationState.WantsDeactivation;

-                                                }

-                                        }

-                                        else

-                                        {

-                                                if (body.ActivationState != ActivationState.DisableDeactivation)

-                                                        body.ActivationState = ActivationState.Active;

-                                        }

-                                }

-                        }

-                }

-

-                protected void UpdateVehicles(float timeStep)

-                {

-                        for (int i = 0; i < _vehicles.Count; i++)

-                        {

-                                RaycastVehicle vehicle = _vehicles[i];

-                                vehicle.updateVehicle(timeStep);

-                        }

-                }

-

-                protected void StartProfiling(float timeStep) { }

-

-                protected virtual void InternalSingleStepSimulation(float timeStep)

-                {

-                        StartProfiling(timeStep);

-

-                        //update aabbs information

-                        UpdateAabbs();

-

-                        //apply gravity, predict motion

-                        PredictUnconstraintMotion(timeStep);

-

-                        DispatcherInfo dispatchInfo = DispatchInfo;

-                        dispatchInfo.TimeStep = timeStep;

-                        dispatchInfo.StepCount = 0;

-                        dispatchInfo.DebugDraw = DebugDrawer;

-

-                        //perform collision detection

-                        PerformDiscreteCollisionDetection();

-

-                        CalculateSimulationIslands();

-

-                        SolverInfo.TimeStep = timeStep;

-

-                        //solve contact and other joint constraints

-                        SolveConstraints(SolverInfo);

-

-                        //CallbackTriggers();

-

-                        //integrate transforms

-                        IntegrateTransforms(timeStep);

-

-                        //update vehicle simulation

-                        UpdateVehicles(timeStep);

-

-                        UpdateActivationState(timeStep);

-                }

-

-                protected void SynchronizeMotionStates()

-                {

-                        //debug vehicle wheels

-                        {

-                                //todo: iterate over awake simulation islands!

-                                for (int i = 0; i < CollisionObjects.Count; i++)

-                                {

-                                        CollisionObject colObj = CollisionObjects[i];

-                                        if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0)

-                                        {

-                                                Vector3 color = new Vector3(255f, 255f, 255f);

-                                                switch (colObj.ActivationState)

-                                                {

-                                                        case ActivationState.Active:

-                                                                color = new Vector3(255f, 255f, 255f); break;

-                                                        case ActivationState.IslandSleeping:

-                                                                color = new Vector3(0f, 255f, 0f); break;

-                                                        case ActivationState.WantsDeactivation:

-                                                                color = new Vector3(0f, 255f, 255f); break;

-                                                        case ActivationState.DisableDeactivation:

-                                                                color = new Vector3(255f, 0f, 0f); break;

-                                                        case ActivationState.DisableSimulation:

-                                                                color = new Vector3(255f, 255f, 0f); break;

-                                                        default:

-                                                                color = new Vector3(255f, 0f, 0f); break;

-                                                }

-

-                                                DebugDrawObject(colObj.WorldTransform, colObj.CollisionShape, color);

-                                        }

-                                        RigidBody body = RigidBody.Upcast(colObj);

-                                        if (body != null && body.MotionState != null && !body.IsStaticOrKinematicObject)

-                                        {

-                                                //if (body.ActivationState != ActivationState.IslandSleeping)

-                                                {

-                                                        Matrix interpolatedTransform = new Matrix();

-                                                        TransformUtil.IntegrateTransform(body.InterpolationWorldTransform,

-                                                                body.InterpolationLinearVelocity, body.InterpolationAngularVelocity, _localTime, ref interpolatedTransform);

-                                                        body.MotionState.SetWorldTransform(interpolatedTransform);

-                                                }

-                                        }

-                                }

-                        }

-

-                        if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0)

-                        {

-                                for (int i = 0; i < _vehicles.Count; i++)

-                                {

-                                        for (int v = 0; v < _vehicles[i].getNumWheels(); v++)

-                                        {

-                                                Vector3 wheelColor = new Vector3(0, 255, 255);

-                                                if (_vehicles[i].getWheelInfo(v).RaycastInfo.IsInContact)

-                                                {

-                                                        wheelColor = new Vector3(0, 0, 255);

-                                                }

-                                                else

-                                                {

-                                                        wheelColor = new Vector3(255, 0, 255);

-                                                }

-

-                                                //synchronize the wheels with the (interpolated) chassis worldtransform

-                                                _vehicles[i].updateWheelTransform(v, true);

-

-                                                Vector3 wheelPosWS = _vehicles[i].getWheelInfo(v).WorldTransform.Translation;

-

-                                                Vector3 axle = new Vector3(

-                                                        MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 0, _vehicles[i].getRightAxis()),

-                                                        MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 1, _vehicles[i].getRightAxis()),

-                                                        MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 2, _vehicles[i].getRightAxis()));

-

-

-                                                //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS

-                                                //debug wheels (cylinders)

-                                                _debugDrawer.DrawLine(wheelPosWS, wheelPosWS + axle, wheelColor);

-                                                _debugDrawer.DrawLine(wheelPosWS, _vehicles[i].getWheelInfo(v).RaycastInfo.ContactPointWS, wheelColor);

-                                        }

-                                }

-                        }

-                }

-

-                protected void SaveKinematicState(float timeStep)

-                {

-                        for (int i = 0; i < CollisionObjects.Count; i++)

-                        {

-                                CollisionObject colObj = CollisionObjects[i];

-                                RigidBody body = RigidBody.Upcast(colObj);

-                                if (body != null)

-                                {

-                                        if (body.ActivationState != ActivationState.IslandSleeping)

-                                        {

-                                                if (body.IsKinematicObject)

-                                                {

-                                                        //to calculate velocities next frame

-                                                        body.SaveKinematicState(timeStep);

-                                                }

-                                        }

-                                }

-                        }

-                }

-

-                internal class InplaceSolverIslandCallback : SimulationIslandManager.IIslandCallback

-                {

-                        private ContactSolverInfo _solverInfo;

-                        private IConstraintSolver _solver;

-                        private IDebugDraw _debugDrawer;

-                        private List<TypedConstraint> _sortedConstraints;

-

-                        public InplaceSolverIslandCallback(

-                                ContactSolverInfo solverInfo,

-                                IConstraintSolver solver,

-                                List<TypedConstraint> sortedConstraints,

-                                IDebugDraw debugDrawer)

-                        {

-                                _solverInfo = solverInfo;

-                                _solver = solver;

-                                _sortedConstraints = sortedConstraints;

-                                _debugDrawer = debugDrawer;

-                        }

-

-                        public ContactSolverInfo SolverInfo { get { return _solverInfo; } set { _solverInfo = value; } }

-                        public IConstraintSolver Solver { get { return _solver; } set { _solver = value; } }

-                        public List<TypedConstraint> Constraints { get { return _sortedConstraints; } set { _sortedConstraints = value; } }

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

-

-                        public void ProcessIsland(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, int islandID)

-                        {

-                                //also add all non-contact constraints/joints for this island

-                                List<TypedConstraint> startConstraint = new List<TypedConstraint>();

-                                int numCurConstraints = 0;

-                                int startIndex = 0;

-                                int i;

-

-                                //find the first constraint for this island

-                                for (i = 0; i < _sortedConstraints.Count; i++)

-                                {

-                                        if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID)

-                                        {

-                                                //startConstraint = &m_sortedConstraints[i];

-                                                startIndex = i;

-                                                break;

-                                        }

-                                }

-                                //count the number of constraints in this island

-                                for (; i < _sortedConstraints.Count; i++)

-                                {

-                                        if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID)

-                                        {

-                                                numCurConstraints++;

-                                        }

-                                }

-

-                                for (i = startIndex; i < startIndex + numCurConstraints; i++)

-                                {

-                                        startConstraint.Add(_sortedConstraints[i]);

-                                }

-

-                                _solver.SolveGroup(bodies, manifolds, numManifolds, startConstraint, _solverInfo, _debugDrawer);

-                        }

-                }

-        }

-

-        internal class DebugDrawCallback : ITriangleIndexCallback, ITriangleCallback

-        {

-                private IDebugDraw _debugDrawer;

-                private Vector3 _color;

-                private Matrix _worldTrans;

-

-                public DebugDrawCallback(IDebugDraw debugDrawer, Matrix worldTrans, Vector3 color)

-                {

-                        _debugDrawer = debugDrawer;

-                        _worldTrans = worldTrans;

-                        _color = color;

-                }

-

-                public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex)

-                {

-                        ProcessTriangle(triangle, partId, triangleIndex);

-                }

-

-                #region ITriangleCallback Members

-

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

-                {

-                        Vector3 wv0, wv1, wv2;

-                        wv0 = Vector3.TransformNormal(triangle[0], _worldTrans);

-                        wv1 = Vector3.TransformNormal(triangle[1], _worldTrans);

-                        wv2 = Vector3.TransformNormal(triangle[2], _worldTrans);

-                        _debugDrawer.DrawLine(wv0, wv1, _color);

-                        _debugDrawer.DrawLine(wv1, wv2, _color);

-                        _debugDrawer.DrawLine(wv2, wv0, _color);

-                }

-

-                #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.Dynamics
+{
+        /// <summary>
+        /// DiscreteDynamicsWorld provides discrete rigid body simulation
+        /// those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController
+        /// </summary>
+        public class DiscreteDynamicsWorld : DynamicsWorld
+        {
+                private static bool _reportMe = true;
+
+                private IConstraintSolver _constraintSolver;
+                private SimulationIslandManager _islandManager;
+                private List<TypedConstraint> _constraints = new List<TypedConstraint>();
+                private IDebugDraw _debugDrawer;
+                private ContactSolverInfo _solverInfo = new ContactSolverInfo();
+                private Vector3 _gravity;
+                //for variable timesteps
+                private float _localTime;
+                //for variable timesteps
+                private bool _ownsIslandManager;
+                private bool _ownsConstraintSolver;
+                private List<RaycastVehicle> _vehicles = new List<RaycastVehicle>();
+                private int _profileTimings;
+
+                public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache)
+                        : this(dispatcher, pairCache, null) { }
+
+                //this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those
+                public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver)
+                        : base(dispatcher, pairCache)
+                {
+                        _constraintSolver = constraintSolver != null ? constraintSolver : new SequentialImpulseConstraintSolver();
+                        _debugDrawer = null;
+                        _gravity = new Vector3(0, -10, 0);
+                        _localTime = 1f / 60f;
+                        _profileTimings = 0;
+                        _islandManager = new SimulationIslandManager();
+                        _ownsIslandManager = true;
+                        _ownsConstraintSolver = constraintSolver == null;
+                }
+
+                public ContactSolverInfo SolverInfo { get { return _solverInfo; } }
+                public SimulationIslandManager SimulationIslandManager { get { return _islandManager; } }
+                public CollisionWorld CollisionWorld { get { return this; } }
+
+                public override IDebugDraw DebugDrawer
+                {
+                        get
+                        {
+                                return _debugDrawer;
+                        }
+                        set
+                        {
+                                _debugDrawer = value;
+                        }
+                }
+
+                public override Vector3 Gravity
+                {
+                        set
+                        {
+                                _gravity = value;
+                                for (int i = 0; i < CollisionObjects.Count; i++)
+                                {
+                                        CollisionObject colObj = CollisionObjects[i];
+                                        RigidBody body = RigidBody.Upcast(colObj);
+                                        if (body != null)
+                                        {
+                                                body.Gravity = value;
+                                        }
+                                }
+                        }
+                }
+
+                public override IConstraintSolver ConstraintSolver
+                {
+                        set
+                        {
+                                _ownsConstraintSolver = false;
+                                _constraintSolver = value;
+                        }
+                }
+
+                public override int ConstraintsCount
+                {
+                        get
+                        {
+                                return _constraints.Count;
+                        }
+                }
+
+                //if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
+                public override void StepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep)
+                {
+                        int numSimulationSubSteps = 0;
+
+                        if (maxSubSteps != 0)
+                        {
+                                //fixed timestep with interpolation
+                                _localTime += timeStep;
+                                if (_localTime >= fixedTimeStep)
+                                {
+                                        numSimulationSubSteps = (int)(_localTime / fixedTimeStep);
+                                        _localTime -= numSimulationSubSteps * fixedTimeStep;
+                                }
+                        }
+                        else
+                        {
+                                //variable timestep
+                                fixedTimeStep = timeStep;
+                                _localTime = timeStep;
+                                if (Math.Abs(timeStep) < float.Epsilon)
+                                {
+                                        numSimulationSubSteps = 0;
+                                        maxSubSteps = 0;
+                                }
+                                else
+                                {
+                                        numSimulationSubSteps = 1;
+                                        maxSubSteps = 1;
+                                }
+                        }
+
+                        //process some debugging flags
+                        if (DebugDrawer != null)
+                        {
+                                RigidBody.DisableDeactivation = (DebugDrawer.DebugMode & DebugDrawModes.NoDeactivation) != 0;
+                        }
+                        if (numSimulationSubSteps != 0)
+                        {
+
+                                SaveKinematicState(fixedTimeStep);
+
+                                //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
+                                int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps;
+
+                                for (int i = 0; i < clampedSimulationSteps; i++)
+                                {
+                                        InternalSingleStepSimulation(fixedTimeStep);
+                                        SynchronizeMotionStates();
+                                }
+
+                        }
+
+                        SynchronizeMotionStates();
+
+                        //return numSimulationSubSteps;
+                }
+
+                public void StepSimulation(float timeStep, int maxSubSteps)
+                {
+                        StepSimulation(timeStep, maxSubSteps, 1f / 60f);
+                }
+
+                public override void UpdateAabbs()
+                {
+                        Vector3 colorvec = new Vector3(1, 0, 0);
+                        for (int i = 0; i < CollisionObjects.Count; i++)
+                        {
+                                CollisionObject colObj = CollisionObjects[i];
+                                RigidBody body = RigidBody.Upcast(colObj);
+
+                                if (body != null)
+                                {
+                                        //      if (body->IsActive() && (!body->IsStatic()))
+                                        {
+                                                Vector3 minAabb, maxAabb;
+                                                colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb);
+                                                OverlappingPairCache bp = BroadphasePairCache;
+
+                                                //moving objects should be moderately sized, probably something wrong if not
+                                                if (colObj.IsStaticObject || ((maxAabb - minAabb).LengthSquared() < 1e12f))
+                                                {
+                                                        bp.SetAabb(body.Broadphase, minAabb, maxAabb);
+                                                }
+                                                else
+                                                {
+                                                        //something went wrong, investigate
+                                                        //this assert is unwanted in 3D modelers (danger of loosing work)
+                                                        BulletDebug.Assert(false);
+                                                        body.ActivationState = ActivationState.DisableSimulation;
+
+                                                        if (_reportMe)
+                                                        {
+                                                                _reportMe = false;
+                                                                Console.WriteLine("Overflow in AABB, object removed from simulation \n");
+                                                                Console.WriteLine("If you can reproduce this, please email bugs@continuousphysics.com\n");
+                                                                Console.WriteLine("Please include above information, your Platform, version of OS.\n");
+                                                                Console.WriteLine("Thanks.\n");
+                                                        }
+                                                }
+                                                if (_debugDrawer != null && (_debugDrawer.DebugMode & DebugDrawModes.DrawAabb) != 0)
+                                                        DrawAabb(_debugDrawer, minAabb, maxAabb, colorvec);
+                                        }
+                                }
+                        }
+                }
+
+                public override void AddConstraint(TypedConstraint constraint)
+                {
+                        _constraints.Add(constraint);
+                }
+
+                public override void RemoveConstraint(TypedConstraint constraint)
+                {
+                        _constraints.Remove(constraint);
+                }
+
+                public void AddVehicle(RaycastVehicle vehicle)
+                {
+                        _vehicles.Add(vehicle);
+                }
+
+                public void RemoveVehicle(RaycastVehicle vehicle)
+                {
+                        _vehicles.Remove(vehicle);
+                }
+
+                public override void AddRigidBody(RigidBody body)
+                {
+                        if (!body.IsStaticOrKinematicObject)
+                        {
+                                body.Gravity = _gravity;
+                        }
+
+                        if (body.CollisionShape != null)
+                        {
+                                bool isDynamic = !(body.IsStaticObject || body.IsKinematicObject);
+                                BroadphaseProxy.CollisionFilterGroups collisionFilterGroup = isDynamic ? BroadphaseProxy.CollisionFilterGroups.Default : BroadphaseProxy.CollisionFilterGroups.Static;
+                                BroadphaseProxy.CollisionFilterGroups collisionFilterMask = isDynamic ? BroadphaseProxy.CollisionFilterGroups.All : (BroadphaseProxy.CollisionFilterGroups.All ^ BroadphaseProxy.CollisionFilterGroups.Static);
+
+                                AddCollisionObject(body, collisionFilterGroup, collisionFilterMask);
+                        }
+                }
+
+                public override void RemoveRigidBody(RigidBody body)
+                {
+                        RemoveCollisionObject(body);
+                }
+
+                public void DebugDrawObject(Matrix worldTransform, CollisionShape shape, Vector3 color)
+                {
+                        if (shape.ShapeType == BroadphaseNativeTypes.Compound)
+                        {
+                                CompoundShape compoundShape = shape as CompoundShape;
+                                for (int i = compoundShape.ChildShapeCount - 1; i >= 0; i--)
+                                {
+                                        Matrix childTrans = compoundShape.GetChildTransform(i);
+                                        CollisionShape colShape = compoundShape.GetChildShape(i);
+                                        DebugDrawObject(worldTransform * childTrans, colShape, color);
+                                }
+
+                        }
+                        else
+                        {
+                                switch (shape.ShapeType)
+                                {
+
+                                        case BroadphaseNativeTypes.Sphere:
+                                                {
+                                                        SphereShape sphereShape = shape as SphereShape;
+                                                        float radius = sphereShape.Margin;//radius doesn't include the margin, so draw with margin
+                                                        Vector3 start = worldTransform.Translation;
+                                                        DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(radius, 0, 0), worldTransform), color);
+                                                        DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, radius, 0), worldTransform), color);
+                                                        DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, 0, radius), worldTransform), color);
+                                                        //drawSphere                                    
+                                                        break;
+                                                }
+                                        case BroadphaseNativeTypes.MultiSphere:
+                                        case BroadphaseNativeTypes.Cone:
+                                                {
+                                                        ConeShape coneShape = shape as ConeShape;
+                                                        float radius = coneShape.Radius;//+coneShape->getMargin();
+                                                        float height = coneShape.Height;//+coneShape->getMargin();
+                                                        Vector3 start = worldTransform.Translation;
+                                                        DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(radius, 0f, -0.5f * height), worldTransform), color);
+                                                        DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(-radius, 0f, -0.5f * height), worldTransform), color);
+                                                        DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, radius, -0.5f * height), worldTransform), color);
+                                                        DebugDrawer.DrawLine(start + Vector3.TransformNormal(new Vector3(0f, 0f, 0.5f * height), worldTransform), start + Vector3.TransformNormal(new Vector3(0f, -radius, -0.5f * height), worldTransform), color);
+                                                        break;
+                                                }
+                                        case BroadphaseNativeTypes.Cylinder:
+                                                {
+                                                        CylinderShape cylinder = shape as CylinderShape;
+                                                        int upAxis = cylinder.UpAxis;
+                                                        float radius = cylinder.Radius;
+                                                        float halfHeight = MathHelper.GetElement(cylinder.HalfExtents, upAxis);
+                                                        Vector3 start = worldTransform.Translation;
+                                                        Vector3 offsetHeight = new Vector3();
+                                                        MathHelper.SetElement(ref offsetHeight, upAxis, halfHeight);
+                                                        Vector3 offsetRadius = new Vector3();
+                                                        MathHelper.SetElement(ref offsetRadius, (upAxis + 1) % 3,  radius);
+                                                        DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight + offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight + offsetRadius, worldTransform), color);
+                                                        DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight - offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight - offsetRadius, worldTransform), color);
+                                                        break;
+                                                } 
+                                        default:
+                                                {
+                                                        if (shape.ShapeType == BroadphaseNativeTypes.TriangleMesh)
+                                                        {
+                                                                TriangleMeshShape concaveMesh = shape as TriangleMeshShape;
+                                                                //btVector3 aabbMax(1e30f,1e30f,1e30f);
+                                                                //btVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f);
+
+                                                                //todo pass camera, for some culling
+                                                                Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
+                                                                Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
+
+                                                                DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color);
+                                                                concaveMesh.ProcessAllTriangles(drawCallback, aabbMin, aabbMax);
+                                                        }
+
+                                                        if (shape.ShapeType == BroadphaseNativeTypes.ConvexTriangleMesh)
+                                                        {
+                                                                ConvexTriangleMeshShape convexMesh = shape as ConvexTriangleMeshShape;
+                                                                //todo: pass camera for some culling                    
+                                                                Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
+                                                                Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
+                                                                //DebugDrawcallback drawCallback;
+                                                                DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color);
+                                                                convexMesh.getStridingMesh().InternalProcessAllTriangles(drawCallback, aabbMin, aabbMax);
+                                                        }
+
+                                                        // for polyhedral shapes
+                                                        if (shape.IsPolyhedral)
+                                                        {
+                                                                PolyhedralConvexShape polyshape = shape as PolyhedralConvexShape;
+
+                                                                for (int i = 0; i < polyshape.EdgeCount; i++)
+                                                                {
+                                                                        Vector3 a, b;
+                                                                        polyshape.GetEdge(i, out a, out b);
+                                                                        a = Vector3.TransformNormal(a, worldTransform);
+                                                                        b = Vector3.TransformNormal(b, worldTransform);
+                                                                        DebugDrawer.DrawLine(a, b, color);
+                                                                }
+                                                        }
+                                                        break;
+                                                }
+                                }
+                        }
+                }
+
+                public override TypedConstraint GetConstraint(int index)
+                {
+                        return _constraints[index];
+                }
+
+                public static void DrawAabb(IDebugDraw debugDrawer, Vector3 from, Vector3 to, Vector3 color)
+                {
+                        Vector3 halfExtents = (to - from) * 0.5f;
+                        Vector3 center = (to + from) * 0.5f;
+
+                        Vector3 edgecoord = new Vector3(1f, 1f, 1f), pa, pb;
+                        for (int i = 0; i < 4; i++)
+                        {
+                                for (int j = 0; j < 3; j++)
+                                {
+                                        pa = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y,
+                                                edgecoord.Z * halfExtents.Z);
+                                        pa += center;
+
+                                        int othercoord = j % 3;
+                                        MathHelper.SetElement(ref edgecoord, othercoord, MathHelper.GetElement(edgecoord, othercoord) * -1f);
+                                        pb = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y,
+                                                edgecoord.Z * halfExtents.Z);
+                                        pb += center;
+
+                                        debugDrawer.DrawLine(pa, pb, color);
+                                }
+                                edgecoord = new Vector3(-1f, -1f, -1f);
+                                if (i < 3)
+                                        MathHelper.SetElement(ref edgecoord, i, MathHelper.GetElement(edgecoord, i) * -1f);
+                        }
+                }
+
+                protected void PredictUnconstraintMotion(float timeStep)
+                {
+                        for (int i = 0; i < CollisionObjects.Count; i++)
+                        {
+                                CollisionObject colObj = CollisionObjects[i];
+                                RigidBody body = RigidBody.Upcast(colObj);
+                                if (body != null)
+                                {
+                                        if (!body.IsStaticOrKinematicObject)
+                                        {
+                                                if (body.IsActive)
+                                                {
+                                                        body.ApplyForces(timeStep);
+                                                        body.IntegrateVelocities(timeStep);
+                                                        Matrix temp = body.InterpolationWorldTransform;
+                                                        body.PredictIntegratedTransform(timeStep, ref temp);
+                                                        body.InterpolationWorldTransform = temp;
+                                                }
+                                        }
+                                }
+                        }
+                }
+
+                protected void IntegrateTransforms(float timeStep)
+                {
+                        Matrix predictedTrans = new Matrix();
+                        for (int i = 0; i < CollisionObjects.Count; i++)
+                        {
+                                CollisionObject colObj = CollisionObjects[i];
+                                RigidBody body = RigidBody.Upcast(colObj);
+                                if (body != null)
+                                {
+                                        if (body.IsActive && (!body.IsStaticOrKinematicObject))
+                                        {
+                                                body.PredictIntegratedTransform(timeStep, ref predictedTrans);
+                                                body.ProceedToTransform(predictedTrans);
+                                        }
+                                }
+                        }
+                }
+
+                protected void CalculateSimulationIslands()
+                {
+                        SimulationIslandManager.UpdateActivationState(this, Dispatcher);
+
+                        for (int i = 0; i < _constraints.Count; i++)
+                        {
+                                TypedConstraint constraint = _constraints[i];
+
+                                RigidBody colObj0 = constraint.RigidBodyA;
+                                RigidBody colObj1 = constraint.RigidBodyB;
+
+                                if (((colObj0 != null) && (colObj0.MergesSimulationIslands)) &&
+                                        ((colObj1 != null) && (colObj1.MergesSimulationIslands)))
+                                {
+                                        if (colObj0.IsActive || colObj1.IsActive)
+                                        {
+
+                                                SimulationIslandManager.UnionFind.Unite((colObj0).IslandTag,
+                                                        (colObj1).IslandTag);
+                                        }
+                                }
+                        }
+
+                        //Store the island id in each body
+                        SimulationIslandManager.StoreIslandActivationState(this);
+                }
+
+                //protected void SolveNonContactConstraints(ContactSolverInfo solverInfo)
+                //{
+                //    //constraint preparation: building jacobians
+                //    for (int i = 0; i < _constraints.Count; i++)
+                //    {
+                //        TypedConstraint constraint = _constraints[i];
+                //        constraint.BuildJacobian();
+                //    }
+
+                //    //solve the regular non-contact constraints (point 2 point, hinge, generic d6)
+                //    for (int g = 0; g < solverInfo.IterationsCount; g++)
+                //    {
+                //        for (int i = 0; i < _constraints.Count; i++)
+                //        {
+                //            TypedConstraint constraint = _constraints[i];
+                //            constraint.SolveConstraint(solverInfo.TimeStep);
+                //        }
+                //    }
+                //}
+
+                //protected void SolveContactConstraints(ContactSolverInfo solverInfo)
+                //{
+                //    InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, _debugDrawer);
+
+                //    // solve all the contact points and contact friction
+                //    _islandManager.BuildAndProcessIslands(Dispatcher, CollisionObjects, solverCallback);
+                //}
+
+                protected void SolveConstraints(ContactSolverInfo solverInfo)
+                {
+                        //sorted version of all btTypedConstraint, based on islandId
+                        List<TypedConstraint> sortedConstraints = new List<TypedConstraint>(ConstraintsCount);
+
+                        for (int i = 0; i < ConstraintsCount; i++)
+                        {
+                                sortedConstraints.Add(_constraints[i]);
+                        }
+
+                        sortedConstraints.Sort(new Comparison<TypedConstraint>(TypedConstraint.SortConstraintOnIslandPredicate));
+                        List<TypedConstraint> constraintsPtr = ConstraintsCount != 0 ? sortedConstraints : new List<TypedConstraint>();
+
+                        InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, constraintsPtr,  _debugDrawer);
+
+                        // solve all the constraints for this island
+                        _islandManager.BuildAndProcessIslands(CollisionWorld.Dispatcher, CollisionWorld.CollisionObjects, solverCallback);
+                }
+
+                protected void UpdateActivationState(float timeStep)
+                {
+                        for (int i = 0; i < CollisionObjects.Count; i++)
+                        {
+                                CollisionObject colObj = CollisionObjects[i];
+                                RigidBody body = RigidBody.Upcast(colObj);
+                                if (body != null)
+                                {
+                                        body.UpdateDeactivation(timeStep);
+
+                                        if (body.WantsSleeping())
+                                        {
+                                                if (body.IsStaticOrKinematicObject)
+                                                {
+                                                        body.ActivationState = ActivationState.IslandSleeping;
+                                                }
+                                                else
+                                                {
+                                                        if (body.ActivationState == ActivationState.Active)
+                                                                body.ActivationState = ActivationState.WantsDeactivation;
+                                                }
+                                        }
+                                        else
+                                        {
+                                                if (body.ActivationState != ActivationState.DisableDeactivation)
+                                                        body.ActivationState = ActivationState.Active;
+                                        }
+                                }
+                        }
+                }
+
+                protected void UpdateVehicles(float timeStep)
+                {
+                        for (int i = 0; i < _vehicles.Count; i++)
+                        {
+                                RaycastVehicle vehicle = _vehicles[i];
+                                vehicle.updateVehicle(timeStep);
+                        }
+                }
+
+                protected void StartProfiling(float timeStep) { }
+
+                protected virtual void InternalSingleStepSimulation(float timeStep)
+                {
+                        StartProfiling(timeStep);
+
+                        //update aabbs information
+                        UpdateAabbs();
+
+                        //apply gravity, predict motion
+                        PredictUnconstraintMotion(timeStep);
+
+                        DispatcherInfo dispatchInfo = DispatchInfo;
+                        dispatchInfo.TimeStep = timeStep;
+                        dispatchInfo.StepCount = 0;
+                        dispatchInfo.DebugDraw = DebugDrawer;
+
+                        //perform collision detection
+                        PerformDiscreteCollisionDetection();
+
+                        CalculateSimulationIslands();
+
+                        SolverInfo.TimeStep = timeStep;
+
+                        //solve contact and other joint constraints
+                        SolveConstraints(SolverInfo);
+
+                        //CallbackTriggers();
+
+                        //integrate transforms
+                        IntegrateTransforms(timeStep);
+
+                        //update vehicle simulation
+                        UpdateVehicles(timeStep);
+
+                        UpdateActivationState(timeStep);
+                }
+
+                protected void SynchronizeMotionStates()
+                {
+                        //debug vehicle wheels
+                        {
+                                //todo: iterate over awake simulation islands!
+                                for (int i = 0; i < CollisionObjects.Count; i++)
+                                {
+                                        CollisionObject colObj = CollisionObjects[i];
+                                        if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0)
+                                        {
+                                                Vector3 color = new Vector3(255f, 255f, 255f);
+                                                switch (colObj.ActivationState)
+                                                {
+                                                        case ActivationState.Active:
+                                                                color = new Vector3(255f, 255f, 255f); break;
+                                                        case ActivationState.IslandSleeping:
+                                                                color = new Vector3(0f, 255f, 0f); break;
+                                                        case ActivationState.WantsDeactivation:
+                                                                color = new Vector3(0f, 255f, 255f); break;
+                                                        case ActivationState.DisableDeactivation:
+                                                                color = new Vector3(255f, 0f, 0f); break;
+                                                        case ActivationState.DisableSimulation:
+                                                                color = new Vector3(255f, 255f, 0f); break;
+                                                        default:
+                                                                color = new Vector3(255f, 0f, 0f); break;
+                                                }
+
+                                                DebugDrawObject(colObj.WorldTransform, colObj.CollisionShape, color);
+                                        }
+                                        RigidBody body = RigidBody.Upcast(colObj);
+                                        if (body != null && body.MotionState != null && !body.IsStaticOrKinematicObject)
+                                        {
+                                                //if (body.ActivationState != ActivationState.IslandSleeping)
+                                                {
+                                                        Matrix interpolatedTransform = new Matrix();
+                                                        TransformUtil.IntegrateTransform(body.InterpolationWorldTransform,
+                                                                body.InterpolationLinearVelocity, body.InterpolationAngularVelocity, _localTime, ref interpolatedTransform);
+                                                        body.MotionState.SetWorldTransform(interpolatedTransform);
+                                                }
+                                        }
+                                }
+                        }
+
+                        if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0)
+                        {
+                                for (int i = 0; i < _vehicles.Count; i++)
+                                {
+                                        for (int v = 0; v < _vehicles[i].getNumWheels(); v++)
+                                        {
+                                                Vector3 wheelColor = new Vector3(0, 255, 255);
+                                                if (_vehicles[i].getWheelInfo(v).RaycastInfo.IsInContact)
+                                                {
+                                                        wheelColor = new Vector3(0, 0, 255);
+                                                }
+                                                else
+                                                {
+                                                        wheelColor = new Vector3(255, 0, 255);
+                                                }
+
+                                                //synchronize the wheels with the (interpolated) chassis worldtransform
+                                                _vehicles[i].updateWheelTransform(v, true);
+
+                                                Vector3 wheelPosWS = _vehicles[i].getWheelInfo(v).WorldTransform.Translation;
+
+                                                Vector3 axle = new Vector3(
+                                                        MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 0, _vehicles[i].getRightAxis()),
+                                                        MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 1, _vehicles[i].getRightAxis()),
+                                                        MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 2, _vehicles[i].getRightAxis()));
+
+
+                                                //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
+                                                //debug wheels (cylinders)
+                                                _debugDrawer.DrawLine(wheelPosWS, wheelPosWS + axle, wheelColor);
+                                                _debugDrawer.DrawLine(wheelPosWS, _vehicles[i].getWheelInfo(v).RaycastInfo.ContactPointWS, wheelColor);
+                                        }
+                                }
+                        }
+                }
+
+                protected void SaveKinematicState(float timeStep)
+                {
+                        for (int i = 0; i < CollisionObjects.Count; i++)
+                        {
+                                CollisionObject colObj = CollisionObjects[i];
+                                RigidBody body = RigidBody.Upcast(colObj);
+                                if (body != null)
+                                {
+                                        if (body.ActivationState != ActivationState.IslandSleeping)
+                                        {
+                                                if (body.IsKinematicObject)
+                                                {
+                                                        //to calculate velocities next frame
+                                                        body.SaveKinematicState(timeStep);
+                                                }
+                                        }
+                                }
+                        }
+                }
+
+                internal class InplaceSolverIslandCallback : SimulationIslandManager.IIslandCallback
+                {
+                        private ContactSolverInfo _solverInfo;
+                        private IConstraintSolver _solver;
+                        private IDebugDraw _debugDrawer;
+                        private List<TypedConstraint> _sortedConstraints;
+
+                        public InplaceSolverIslandCallback(
+                                ContactSolverInfo solverInfo,
+                                IConstraintSolver solver,
+                                List<TypedConstraint> sortedConstraints,
+                                IDebugDraw debugDrawer)
+                        {
+                                _solverInfo = solverInfo;
+                                _solver = solver;
+                                _sortedConstraints = sortedConstraints;
+                                _debugDrawer = debugDrawer;
+                        }
+
+                        public ContactSolverInfo SolverInfo { get { return _solverInfo; } set { _solverInfo = value; } }
+                        public IConstraintSolver Solver { get { return _solver; } set { _solver = value; } }
+                        public List<TypedConstraint> Constraints { get { return _sortedConstraints; } set { _sortedConstraints = value; } }
+                        public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } }
+
+                        public void ProcessIsland(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, int islandID)
+                        {
+                                //also add all non-contact constraints/joints for this island
+                                List<TypedConstraint> startConstraint = new List<TypedConstraint>();
+                                int numCurConstraints = 0;
+                                int startIndex = 0;
+                                int i;
+
+                                //find the first constraint for this island
+                                for (i = 0; i < _sortedConstraints.Count; i++)
+                                {
+                                        if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID)
+                                        {
+                                                //startConstraint = &m_sortedConstraints[i];
+                                                startIndex = i;
+                                                break;
+                                        }
+                                }
+                                //count the number of constraints in this island
+                                for (; i < _sortedConstraints.Count; i++)
+                                {
+                                        if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID)
+                                        {
+                                                numCurConstraints++;
+                                        }
+                                }
+
+                                for (i = startIndex; i < startIndex + numCurConstraints; i++)
+                                {
+                                        startConstraint.Add(_sortedConstraints[i]);
+                                }
+
+                                _solver.SolveGroup(bodies, manifolds, numManifolds, startConstraint, _solverInfo, _debugDrawer);
+                        }
+                }
+        }
+
+        internal class DebugDrawCallback : ITriangleIndexCallback, ITriangleCallback
+        {
+                private IDebugDraw _debugDrawer;
+                private Vector3 _color;
+                private Matrix _worldTrans;
+
+                public DebugDrawCallback(IDebugDraw debugDrawer, Matrix worldTrans, Vector3 color)
+                {
+                        _debugDrawer = debugDrawer;
+                        _worldTrans = worldTrans;
+                        _color = color;
+                }
+
+                public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex)
+                {
+                        ProcessTriangle(triangle, partId, triangleIndex);
+                }
+
+                #region ITriangleCallback Members
+
+                public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex)
+                {
+                        Vector3 wv0, wv1, wv2;
+                        wv0 = Vector3.TransformNormal(triangle[0], _worldTrans);
+                        wv1 = Vector3.TransformNormal(triangle[1], _worldTrans);
+                        wv2 = Vector3.TransformNormal(triangle[2], _worldTrans);
+                        _debugDrawer.DrawLine(wv0, wv1, _color);
+                        _debugDrawer.DrawLine(wv1, wv2, _color);
+                        _debugDrawer.DrawLine(wv2, wv0, _color);
+                }
+
+                #endregion
+        }
 }
\ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs
index 647c8ec..34ae608 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs
@@ -1,59 +1,59 @@
-/*

-  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.Dynamics

-{

-        public abstract class DynamicsWorld : CollisionWorld

-        {

-                public DynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache)

-                        : base(dispatcher, pairCache) { }

-

-                //once a rigidbody is added to the dynamics world, it will get this gravity assigned

-                //existing rigidbodies in the world get gravity assigned too, during this method

-                public abstract Vector3 Gravity { set; }

-                public abstract IConstraintSolver ConstraintSolver { set; }

-                public virtual int ConstraintsCount { get { return 0; } }

-                public abstract IDebugDraw DebugDrawer { get; set; }

-

-                //stepSimulation proceeds the simulation over timeStep units

-                public abstract void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep);

-

-                public void StepSimulation(float timeStep)

-                {

-                        StepSimulation(timeStep, 1, 1f / 60f);

-                }

-

-                public abstract void UpdateAabbs();

-

-                public virtual void AddConstraint(TypedConstraint constraint) { }

-                public virtual void RemoveConstraint(TypedConstraint constraint) { }

-

-                public abstract void AddRigidBody(RigidBody body);

-                public abstract void RemoveRigidBody(RigidBody body);

-

-                public virtual TypedConstraint GetConstraint(int index) { return null; }

-        }

-}

+/*
+  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.Dynamics
+{
+        public abstract class DynamicsWorld : CollisionWorld
+        {
+                public DynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache)
+                        : base(dispatcher, pairCache) { }
+
+                //once a rigidbody is added to the dynamics world, it will get this gravity assigned
+                //existing rigidbodies in the world get gravity assigned too, during this method
+                public abstract Vector3 Gravity { set; }
+                public abstract IConstraintSolver ConstraintSolver { set; }
+                public virtual int ConstraintsCount { get { return 0; } }
+                public abstract IDebugDraw DebugDrawer { get; set; }
+
+                //stepSimulation proceeds the simulation over timeStep units
+                public abstract void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep);
+
+                public void StepSimulation(float timeStep)
+                {
+                        StepSimulation(timeStep, 1, 1f / 60f);
+                }
+
+                public abstract void UpdateAabbs();
+
+                public virtual void AddConstraint(TypedConstraint constraint) { }
+                public virtual void RemoveConstraint(TypedConstraint constraint) { }
+
+                public abstract void AddRigidBody(RigidBody body);
+                public abstract void RemoveRigidBody(RigidBody body);
+
+                public virtual TypedConstraint GetConstraint(int index) { return null; }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs
index ec14daf..12a7485 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs
@@ -1,447 +1,447 @@
-/*

-  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.Dynamics

-{

-        public class RigidBody : CollisionObject

-        {

-                private static float _linearAirDamping = 1;

-                //'temporarily' global variables

-                private static float _rigidBodyDeactivationTime = 2;

-                private static bool _disableDeactivation = false;

-

-                private static float _linearSleepingThreshold = 0.8f;

-                private static float _angularSleepingThreshold = 1.0f;

-                private static int _uniqueId = 0;

-

-                private Matrix _invInertiaTensorWorld;

-                private Vector3 _linearVelocity;

-                private Vector3 _angularVelocity;

-                private float _inverseMass;

-                private float _angularFactor;

-

-                private Vector3 _gravity;

-                private Vector3 _invInertiaLocal;

-                private Vector3 _totalForce;

-                private Vector3 _totalTorque;

-

-                private float _linearDamping;

-                private float _angularDamping;

-

-                //m_optionalMotionState allows to automatic synchronize the world transform for active objects

-                private MotionState _optionalMotionState;

-

-                //for experimental overriding of friction/contact solver func

-                private ContactSolverType _contactSolverType;

-                private ContactSolverType _frictionSolverType;

-

-                private int _debugBodyId;

-

-                //Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate

-                //doesn't work very well yet (value 0 disabled this damping)

-                //note there this influences deactivation thresholds!

-                private float _clippedAngvelThresholdSqr = 0.01f;

-                private float _clippedLinearThresholdSqr = 0.01f;

-

-                private float _jitterVelocityDampingFactor = 0.7f;

-

-                public RigidBody(float mass, MotionState motionState, CollisionShape collisionShape, Vector3 localInertia, float linearDamping, float angularDamping, float friction, float restitution)

-                {

-                        _optionalMotionState = motionState;

-                        _angularFactor = 1;

-                        _angularDamping = 0.5f;

-

-                        if (motionState != null)

-                        {

-                                motionState.GetWorldTransform(out _worldTransform);

-                        }

-                        else

-                        {

-                                WorldTransform = Matrix.Identity;

-                        }

-

-                        InterpolationWorldTransform = WorldTransform;

-                        InterpolationLinearVelocity = new Vector3();

-                        InterpolationAngularVelocity = new Vector3();

-

-                        //moved to btCollisionObject

-                        Friction = friction;

-                        Restitution = restitution;

-

-                        CollisionShape = collisionShape;

-                        _debugBodyId = UniqueID++;

-

-                        //m_internalOwner is to allow upcasting from collision object to rigid body

-                        Owner = this;

-

-                        SetMassProps(mass, localInertia);

-                        SetDamping(linearDamping, angularDamping);

-                        UpdateInertiaTensor();

-                }

-

-                public int DebugBodyID { get { return _debugBodyId; } set { _debugBodyId = value; } }

-

-                public ContactSolverType ContactSolverType { get { return _contactSolverType; } set { _contactSolverType = value; } }

-                public ContactSolverType FrictionSolverType { get { return _frictionSolverType; } set { _frictionSolverType = value; } }

-

-                public float AngularFactor { get { return _angularFactor; } set { _angularFactor = value; } }

-

-                //is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase?

-                public bool IsInWorld { get { return Broadphase != null; } }

-

-                public Vector3 Gravity

-                {

-                        get { return _gravity; }

-                        set

-                        {

-                                if (_inverseMass != 0.0f)

-                                {

-                                        _gravity = value * (1.0f / _inverseMass);

-                                }

-                        }

-                }

-                public Matrix InvInertiaTensorWorld { get { return _invInertiaTensorWorld; } }

-                public float InverseMass { get { return _inverseMass; } }

-                public Vector3 InvInertiaDiagLocal { get { return _invInertiaLocal; } set { _invInertiaLocal = value; } }

-                public Vector3 CenterOfMassPosition { get { return WorldTransform.Translation; } }

-                public Quaternion Orientation { get { return Quaternion.CreateFromRotationMatrix(WorldTransform); } }

-                public Matrix CenterOfMassTransform

-                {

-                        get { return WorldTransform; }

-                        set

-                        {

-                                InterpolationWorldTransform = value;

-                                InterpolationLinearVelocity = LinearVelocity;

-                                InterpolationAngularVelocity = AngularVelocity;

-                                WorldTransform = value;

-                                UpdateInertiaTensor();

-                        }

-                }

-

-                public Vector3 LinearVelocity

-                {

-                        get { return _linearVelocity; }

-                        set

-                        {

-                                if (CollisionFlags == CollisionOptions.StaticObject)

-                                        throw new BulletException("Static objects can't have linear velocity!");

-                                _linearVelocity = value;

-                        }

-                }

-

-                public Vector3 AngularVelocity

-                {

-                        get { return _angularVelocity; }

-                        set

-                        {

-                                if (CollisionFlags == CollisionOptions.StaticObject)

-                                        throw new BulletException("Static objects can't have angular velocity!");

-                                _angularVelocity = value;

-                        }

-                }

-

-                //MotionState allows to automatic synchronize the world transform for active objects

-                public MotionState MotionState

-                {

-                        get { return _optionalMotionState; }

-                        set

-                        {

-                                _optionalMotionState = value;

-                                if (_optionalMotionState != null)

-                                        value.GetWorldTransform(out _worldTransform);

-                        }

-                }

-

-                public static float LinearAirDamping { get { return _linearAirDamping; } set { _linearAirDamping = value; } }

-                public static float RigidBodyDeactivationTime { get { return _rigidBodyDeactivationTime; } set { _rigidBodyDeactivationTime = value; } }

-                public static bool DisableDeactivation { get { return _disableDeactivation; } set { _disableDeactivation = value; } }

-                public static float LinearSleepingThreshold { get { return _linearSleepingThreshold; } set { _linearSleepingThreshold = value; } }

-                public static float AngularSleepingThreshold { get { return _angularSleepingThreshold; } set { _angularSleepingThreshold = value; } }

-                public static int UniqueID { get { return _uniqueId; } set { _uniqueId = value; } }

-

-                public void ProceedToTransform(Matrix newTrans)

-                {

-                        CenterOfMassTransform = newTrans;

-                }

-

-                //to keep collision detection and dynamics separate we don't store a rigidbody pointer

-                //but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast

-                public static RigidBody Upcast(CollisionObject colObj)

-                {

-                        return colObj.Owner as RigidBody;

-                }

-

-                // continuous collision detection needs prediction

-                public void PredictIntegratedTransform(float step, ref Matrix predictedTransform)

-                {

-                        if ((_angularVelocity.LengthSquared() < _clippedAngvelThresholdSqr) &&

-                                (_linearVelocity.LengthSquared() < _clippedLinearThresholdSqr))

-                        {

-                                _angularVelocity *= _jitterVelocityDampingFactor;

-                                _linearVelocity *= _jitterVelocityDampingFactor;

-                        }

-

-                        TransformUtil.IntegrateTransform(WorldTransform, _linearVelocity, _angularVelocity, step, ref predictedTransform);

-                }

-

-                public void SaveKinematicState(float step)

-                {

-                        //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities

-                        if (step != 0)

-                        {

-                                //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform

-                                if (MotionState != null)

-                                        MotionState.GetWorldTransform(out _worldTransform);

-

-                                TransformUtil.CalculateVelocity(InterpolationWorldTransform, WorldTransform, step, ref _linearVelocity, ref _angularVelocity);

-                                InterpolationLinearVelocity = _linearVelocity;

-                                InterpolationAngularVelocity = _angularVelocity;

-                                InterpolationWorldTransform = WorldTransform;

-                        }

-                }

-

-                public void ApplyForces(float step)

-                {

-                        if (IsStaticOrKinematicObject)

-                                return;

-

-                        ApplyCentralForce(_gravity);

-

-                        _linearVelocity *= (1 - step * LinearAirDamping * _linearDamping) < 0.0f ? 0.0f : (1.0f < (1 - step * LinearAirDamping * _linearDamping) ? 1.0f : (1 - step * LinearAirDamping * _linearDamping)); 

-                        _angularVelocity *= (1 - step * _angularDamping) < 0.0f ? 0.0f : (1.0f < (1 - step * _angularDamping) ? 1.0f : (1 - step * _angularDamping)); 

-

-                        float speed = _linearVelocity.Length();

-                        if (speed < _linearDamping)

-                        {

-                                float dampVel = 0.005f;

-                                if (speed > dampVel)

-                                {

-                                        Vector3 dir = _linearVelocity;

-                                        dir.Normalize();

-                                        _linearVelocity -= dir * dampVel;

-                                }

-                                else

-                                {

-                                        _linearVelocity = new Vector3();

-                                }

-                        }

-

-                        float angSpeed = _angularVelocity.Length();

-                        if (angSpeed < _angularDamping)

-                        {

-                                float angDampVel = 0.005f;

-                                if (angSpeed > angDampVel)

-                                {

-                                        Vector3 dir = _angularVelocity;

-                                        dir.Normalize();

-                                        _angularVelocity -= dir * angDampVel;

-                                }

-                                else

-                                {

-                                        _angularVelocity = new Vector3();

-                                }

-                        }

-                }

-

-                public void SetDamping(float linDamping, float angDamping)

-                {

-                        _linearDamping = linDamping < 0.0f ? 0.0f : (1.0f < linDamping ? 1.0f : linDamping); 

-                        _angularDamping = angDamping < 0.0f ? 0.0f : (1.0f < angDamping ? 1.0f : angDamping); 

-                }

-

-                public void SetMassProps(float mass, Vector3 inertia)

-                {

-                        if (mass == 0)

-                        {

-                                CollisionFlags |= CollisionOptions.StaticObject;

-                                _inverseMass = 0;

-                        }

-                        else

-                        {

-                                CollisionFlags &= (~CollisionOptions.StaticObject);

-                                _inverseMass = 1.0f / mass;

-                        }

-

-                        _invInertiaLocal = new Vector3(inertia.X != 0.0f ? 1.0f / inertia.X : 0.0f,

-                                                                   inertia.Y != 0.0f ? 1.0f / inertia.Y : 0.0f,

-                                                                   inertia.Z != 0.0f ? 1.0f / inertia.Z : 0.0f);

-                }

-

-                public void IntegrateVelocities(float step)

-                {

-                        if (IsStaticOrKinematicObject)

-                                return;

-

-                        _linearVelocity += _totalForce * (_inverseMass * step);

-                        _angularVelocity += Vector3.TransformNormal(_totalTorque, _invInertiaTensorWorld) * step;

-

-            //float MAX_ANGVEL = MonoXnaCompactMaths.MathHelper.PiOver2;

-            float MAX_ANGVEL = (float)Math.PI / 2.0f;

-                        /// clamp angular velocity. collision calculations will fail on higher angular velocities       

-                        float angvel = _angularVelocity.Length();

-                        if (angvel * step > MAX_ANGVEL)

-                        {

-                                _angularVelocity *= (MAX_ANGVEL / step) / angvel;

-                        }

-

-                        ClearForces();

-                }

-

-                public void ApplyCentralForce(Vector3 force)

-                {

-                        _totalForce += force;

-                }

-

-                public void ApplyTorque(Vector3 torque)

-                {

-                        _totalTorque += torque;

-                }

-

-                public void ApplyForce(Vector3 force, Vector3 rel_pos)

-                {

-                        ApplyCentralForce(force);

-                        ApplyTorque(Vector3.Cross(rel_pos, force));

-                }

-

-                public void ApplyCentralImpulse(Vector3 impulse)

-                {

-                        _linearVelocity += impulse * _inverseMass;

-                }

-

-                public void ApplyTorqueImpulse(Vector3 torque)

-                {

-                        _angularVelocity += Vector3.TransformNormal(torque, _invInertiaTensorWorld);

-                }

-

-                public void ApplyImpulse(Vector3 impulse, Vector3 rel_pos)

-                {

-                        if (_inverseMass != 0)

-                        {

-                                ApplyCentralImpulse(impulse);

-                                if (_angularFactor != 0)

-                                        ApplyTorqueImpulse(Vector3.Cross(rel_pos, impulse) * _angularFactor);

-                        }

-                }

-

-                public void InternalApplyImpulse(Vector3 linearComponent, Vector3 angularComponent, float impulseMagnitude)

-                {

-                        if (_inverseMass != 0)

-                        {

-                                _linearVelocity += linearComponent * impulseMagnitude;

-                                if (_angularFactor != 0)

-                                        _angularVelocity += angularComponent * impulseMagnitude * _angularFactor;

-                        }

-                }

-

-                public void ClearForces()

-                {

-                        _totalForce = new Vector3();

-                        _totalTorque = new Vector3();

-                }

-

-                public void UpdateInertiaTensor()

-                {

-                        Matrix temp = WorldTransform;

-                        temp.Translation = Vector3.Zero;

-                        _invInertiaTensorWorld = MatrixOperations.Multiply(MatrixOperations.Scaled(WorldTransform, _invInertiaLocal), Matrix.Transpose(temp));

-                }

-

-                public Vector3 GetVelocityInLocalPoint(Vector3 relPos)

-                {

-                        //we also calculate lin/ang velocity for kinematic objects

-                        return _linearVelocity + Vector3.Cross(_angularVelocity, relPos);

-

-                        //for kinematic objects, we could also use use:

-                        //              return  (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;

-                }

-

-                public void Translate(Vector3 v)

-                {

-                        Matrix m = WorldTransform;

-                        m.Translation += v;

-                        WorldTransform = m;

-                }

-

-                public void GetAabb(out Vector3 aabbMin, out Vector3 aabbMax)

-                {

-                        CollisionShape.GetAabb(WorldTransform, out aabbMin, out aabbMax);

-                }

-

-                public float ComputeImpulseDenominator(Vector3 pos, Vector3 normal)

-                {

-                        Vector3 r0 = pos - CenterOfMassPosition;

-                        Vector3 c0 = Vector3.Cross(r0, normal);

-                        Vector3 vec = Vector3.Cross(Vector3.TransformNormal(c0, InvInertiaTensorWorld), r0);

-

-                        return _inverseMass + Vector3.Dot(normal, vec);

-

-                }

-

-                public float ComputeAngularImpulseDenominator(Vector3 axis)

-                {

-                        Vector3 vec = Vector3.TransformNormal(axis, InvInertiaTensorWorld);

-                        return Vector3.Dot(axis, vec);

-                }

-

-                public void UpdateDeactivation(float timeStep)

-                {

-                        if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.DisableDeactivation))

-                                return;

-

-                        if ((LinearVelocity.LengthSquared() < LinearSleepingThreshold * LinearSleepingThreshold) &&

-                                (AngularVelocity.LengthSquared() < AngularSleepingThreshold * AngularSleepingThreshold))

-                        {

-                                DeactivationTime += timeStep;

-                        }

-                        else

-                        {

-                                DeactivationTime = 0;

-                                ActivationState = ActivationState.Nothing;

-                        }

-

-                }

-

-                public bool WantsSleeping()

-                {

-

-                        if (ActivationState == ActivationState.DisableDeactivation)

-                                return false;

-

-                        //disable deactivation

-                        if (DisableDeactivation || (RigidBodyDeactivationTime == 0))

-                                return false;

-

-                        if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.WantsDeactivation))

-                                return true;

-

-                        if (DeactivationTime > RigidBodyDeactivationTime)

-                        {

-                                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.Dynamics
+{
+        public class RigidBody : CollisionObject
+        {
+                private static float _linearAirDamping = 1;
+                //'temporarily' global variables
+                private static float _rigidBodyDeactivationTime = 2;
+                private static bool _disableDeactivation = false;
+
+                private static float _linearSleepingThreshold = 0.8f;
+                private static float _angularSleepingThreshold = 1.0f;
+                private static int _uniqueId = 0;
+
+                private Matrix _invInertiaTensorWorld;
+                private Vector3 _linearVelocity;
+                private Vector3 _angularVelocity;
+                private float _inverseMass;
+                private float _angularFactor;
+
+                private Vector3 _gravity;
+                private Vector3 _invInertiaLocal;
+                private Vector3 _totalForce;
+                private Vector3 _totalTorque;
+
+                private float _linearDamping;
+                private float _angularDamping;
+
+                //m_optionalMotionState allows to automatic synchronize the world transform for active objects
+                private MotionState _optionalMotionState;
+
+                //for experimental overriding of friction/contact solver func
+                private ContactSolverType _contactSolverType;
+                private ContactSolverType _frictionSolverType;
+
+                private int _debugBodyId;
+
+                //Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate
+                //doesn't work very well yet (value 0 disabled this damping)
+                //note there this influences deactivation thresholds!
+                private float _clippedAngvelThresholdSqr = 0.01f;
+                private float _clippedLinearThresholdSqr = 0.01f;
+
+                private float _jitterVelocityDampingFactor = 0.7f;
+
+                public RigidBody(float mass, MotionState motionState, CollisionShape collisionShape, Vector3 localInertia, float linearDamping, float angularDamping, float friction, float restitution)
+                {
+                        _optionalMotionState = motionState;
+                        _angularFactor = 1;
+                        _angularDamping = 0.5f;
+
+                        if (motionState != null)
+                        {
+                                motionState.GetWorldTransform(out _worldTransform);
+                        }
+                        else
+                        {
+                                WorldTransform = Matrix.Identity;
+                        }
+
+                        InterpolationWorldTransform = WorldTransform;
+                        InterpolationLinearVelocity = new Vector3();
+                        InterpolationAngularVelocity = new Vector3();
+
+                        //moved to btCollisionObject
+                        Friction = friction;
+                        Restitution = restitution;
+
+                        CollisionShape = collisionShape;
+                        _debugBodyId = UniqueID++;
+
+                        //m_internalOwner is to allow upcasting from collision object to rigid body
+                        Owner = this;
+
+                        SetMassProps(mass, localInertia);
+                        SetDamping(linearDamping, angularDamping);
+                        UpdateInertiaTensor();
+                }
+
+                public int DebugBodyID { get { return _debugBodyId; } set { _debugBodyId = value; } }
+
+                public ContactSolverType ContactSolverType { get { return _contactSolverType; } set { _contactSolverType = value; } }
+                public ContactSolverType FrictionSolverType { get { return _frictionSolverType; } set { _frictionSolverType = value; } }
+
+                public float AngularFactor { get { return _angularFactor; } set { _angularFactor = value; } }
+
+                //is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase?
+                public bool IsInWorld { get { return Broadphase != null; } }
+
+                public Vector3 Gravity
+                {
+                        get { return _gravity; }
+                        set
+                        {
+                                if (_inverseMass != 0.0f)
+                                {
+                                        _gravity = value * (1.0f / _inverseMass);
+                                }
+                        }
+                }
+                public Matrix InvInertiaTensorWorld { get { return _invInertiaTensorWorld; } }
+                public float InverseMass { get { return _inverseMass; } }
+                public Vector3 InvInertiaDiagLocal { get { return _invInertiaLocal; } set { _invInertiaLocal = value; } }
+                public Vector3 CenterOfMassPosition { get { return WorldTransform.Translation; } }
+                public Quaternion Orientation { get { return Quaternion.CreateFromRotationMatrix(WorldTransform); } }
+                public Matrix CenterOfMassTransform
+                {
+                        get { return WorldTransform; }
+                        set
+                        {
+                                InterpolationWorldTransform = value;
+                                InterpolationLinearVelocity = LinearVelocity;
+                                InterpolationAngularVelocity = AngularVelocity;
+                                WorldTransform = value;
+                                UpdateInertiaTensor();
+                        }
+                }
+
+                public Vector3 LinearVelocity
+                {
+                        get { return _linearVelocity; }
+                        set
+                        {
+                                if (CollisionFlags == CollisionOptions.StaticObject)
+                                        throw new BulletException("Static objects can't have linear velocity!");
+                                _linearVelocity = value;
+                        }
+                }
+
+                public Vector3 AngularVelocity
+                {
+                        get { return _angularVelocity; }
+                        set
+                        {
+                                if (CollisionFlags == CollisionOptions.StaticObject)
+                                        throw new BulletException("Static objects can't have angular velocity!");
+                                _angularVelocity = value;
+                        }
+                }
+
+                //MotionState allows to automatic synchronize the world transform for active objects
+                public MotionState MotionState
+                {
+                        get { return _optionalMotionState; }
+                        set
+                        {
+                                _optionalMotionState = value;
+                                if (_optionalMotionState != null)
+                                        value.GetWorldTransform(out _worldTransform);
+                        }
+                }
+
+                public static float LinearAirDamping { get { return _linearAirDamping; } set { _linearAirDamping = value; } }
+                public static float RigidBodyDeactivationTime { get { return _rigidBodyDeactivationTime; } set { _rigidBodyDeactivationTime = value; } }
+                public static bool DisableDeactivation { get { return _disableDeactivation; } set { _disableDeactivation = value; } }
+                public static float LinearSleepingThreshold { get { return _linearSleepingThreshold; } set { _linearSleepingThreshold = value; } }
+                public static float AngularSleepingThreshold { get { return _angularSleepingThreshold; } set { _angularSleepingThreshold = value; } }
+                public static int UniqueID { get { return _uniqueId; } set { _uniqueId = value; } }
+
+                public void ProceedToTransform(Matrix newTrans)
+                {
+                        CenterOfMassTransform = newTrans;
+                }
+
+                //to keep collision detection and dynamics separate we don't store a rigidbody pointer
+                //but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast
+                public static RigidBody Upcast(CollisionObject colObj)
+                {
+                        return colObj.Owner as RigidBody;
+                }
+
+                // continuous collision detection needs prediction
+                public void PredictIntegratedTransform(float step, ref Matrix predictedTransform)
+                {
+                        if ((_angularVelocity.LengthSquared() < _clippedAngvelThresholdSqr) &&
+                                (_linearVelocity.LengthSquared() < _clippedLinearThresholdSqr))
+                        {
+                                _angularVelocity *= _jitterVelocityDampingFactor;
+                                _linearVelocity *= _jitterVelocityDampingFactor;
+                        }
+
+                        TransformUtil.IntegrateTransform(WorldTransform, _linearVelocity, _angularVelocity, step, ref predictedTransform);
+                }
+
+                public void SaveKinematicState(float step)
+                {
+                        //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
+                        if (step != 0)
+                        {
+                                //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
+                                if (MotionState != null)
+                                        MotionState.GetWorldTransform(out _worldTransform);
+
+                                TransformUtil.CalculateVelocity(InterpolationWorldTransform, WorldTransform, step, ref _linearVelocity, ref _angularVelocity);
+                                InterpolationLinearVelocity = _linearVelocity;
+                                InterpolationAngularVelocity = _angularVelocity;
+                                InterpolationWorldTransform = WorldTransform;
+                        }
+                }
+
+                public void ApplyForces(float step)
+                {
+                        if (IsStaticOrKinematicObject)
+                                return;
+
+                        ApplyCentralForce(_gravity);
+
+                        _linearVelocity *= (1 - step * LinearAirDamping * _linearDamping) < 0.0f ? 0.0f : (1.0f < (1 - step * LinearAirDamping * _linearDamping) ? 1.0f : (1 - step * LinearAirDamping * _linearDamping)); 
+                        _angularVelocity *= (1 - step * _angularDamping) < 0.0f ? 0.0f : (1.0f < (1 - step * _angularDamping) ? 1.0f : (1 - step * _angularDamping)); 
+
+                        float speed = _linearVelocity.Length();
+                        if (speed < _linearDamping)
+                        {
+                                float dampVel = 0.005f;
+                                if (speed > dampVel)
+                                {
+                                        Vector3 dir = _linearVelocity;
+                                        dir.Normalize();
+                                        _linearVelocity -= dir * dampVel;
+                                }
+                                else
+                                {
+                                        _linearVelocity = new Vector3();
+                                }
+                        }
+
+                        float angSpeed = _angularVelocity.Length();
+                        if (angSpeed < _angularDamping)
+                        {
+                                float angDampVel = 0.005f;
+                                if (angSpeed > angDampVel)
+                                {
+                                        Vector3 dir = _angularVelocity;
+                                        dir.Normalize();
+                                        _angularVelocity -= dir * angDampVel;
+                                }
+                                else
+                                {
+                                        _angularVelocity = new Vector3();
+                                }
+                        }
+                }
+
+                public void SetDamping(float linDamping, float angDamping)
+                {
+                        _linearDamping = linDamping < 0.0f ? 0.0f : (1.0f < linDamping ? 1.0f : linDamping); 
+                        _angularDamping = angDamping < 0.0f ? 0.0f : (1.0f < angDamping ? 1.0f : angDamping); 
+                }
+
+                public void SetMassProps(float mass, Vector3 inertia)
+                {
+                        if (mass == 0)
+                        {
+                                CollisionFlags |= CollisionOptions.StaticObject;
+                                _inverseMass = 0;
+                        }
+                        else
+                        {
+                                CollisionFlags &= (~CollisionOptions.StaticObject);
+                                _inverseMass = 1.0f / mass;
+                        }
+
+                        _invInertiaLocal = new Vector3(inertia.X != 0.0f ? 1.0f / inertia.X : 0.0f,
+                                                                   inertia.Y != 0.0f ? 1.0f / inertia.Y : 0.0f,
+                                                                   inertia.Z != 0.0f ? 1.0f / inertia.Z : 0.0f);
+                }
+
+                public void IntegrateVelocities(float step)
+                {
+                        if (IsStaticOrKinematicObject)
+                                return;
+
+                        _linearVelocity += _totalForce * (_inverseMass * step);
+                        _angularVelocity += Vector3.TransformNormal(_totalTorque, _invInertiaTensorWorld) * step;
+
+            //float MAX_ANGVEL = MonoXnaCompactMaths.MathHelper.PiOver2;
+            float MAX_ANGVEL = (float)Math.PI / 2.0f;
+                        /// clamp angular velocity. collision calculations will fail on higher angular velocities       
+                        float angvel = _angularVelocity.Length();
+                        if (angvel * step > MAX_ANGVEL)
+                        {
+                                _angularVelocity *= (MAX_ANGVEL / step) / angvel;
+                        }
+
+                        ClearForces();
+                }
+
+                public void ApplyCentralForce(Vector3 force)
+                {
+                        _totalForce += force;
+                }
+
+                public void ApplyTorque(Vector3 torque)
+                {
+                        _totalTorque += torque;
+                }
+
+                public void ApplyForce(Vector3 force, Vector3 rel_pos)
+                {
+                        ApplyCentralForce(force);
+                        ApplyTorque(Vector3.Cross(rel_pos, force));
+                }
+
+                public void ApplyCentralImpulse(Vector3 impulse)
+                {
+                        _linearVelocity += impulse * _inverseMass;
+                }
+
+                public void ApplyTorqueImpulse(Vector3 torque)
+                {
+                        _angularVelocity += Vector3.TransformNormal(torque, _invInertiaTensorWorld);
+                }
+
+                public void ApplyImpulse(Vector3 impulse, Vector3 rel_pos)
+                {
+                        if (_inverseMass != 0)
+                        {
+                                ApplyCentralImpulse(impulse);
+                                if (_angularFactor != 0)
+                                        ApplyTorqueImpulse(Vector3.Cross(rel_pos, impulse) * _angularFactor);
+                        }
+                }
+
+                public void InternalApplyImpulse(Vector3 linearComponent, Vector3 angularComponent, float impulseMagnitude)
+                {
+                        if (_inverseMass != 0)
+                        {
+                                _linearVelocity += linearComponent * impulseMagnitude;
+                                if (_angularFactor != 0)
+                                        _angularVelocity += angularComponent * impulseMagnitude * _angularFactor;
+                        }
+                }
+
+                public void ClearForces()
+                {
+                        _totalForce = new Vector3();
+                        _totalTorque = new Vector3();
+                }
+
+                public void UpdateInertiaTensor()
+                {
+                        Matrix temp = WorldTransform;
+                        temp.Translation = Vector3.Zero;
+                        _invInertiaTensorWorld = MatrixOperations.Multiply(MatrixOperations.Scaled(WorldTransform, _invInertiaLocal), Matrix.Transpose(temp));
+                }
+
+                public Vector3 GetVelocityInLocalPoint(Vector3 relPos)
+                {
+                        //we also calculate lin/ang velocity for kinematic objects
+                        return _linearVelocity + Vector3.Cross(_angularVelocity, relPos);
+
+                        //for kinematic objects, we could also use use:
+                        //              return  (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
+                }
+
+                public void Translate(Vector3 v)
+                {
+                        Matrix m = WorldTransform;
+                        m.Translation += v;
+                        WorldTransform = m;
+                }
+
+                public void GetAabb(out Vector3 aabbMin, out Vector3 aabbMax)
+                {
+                        CollisionShape.GetAabb(WorldTransform, out aabbMin, out aabbMax);
+                }
+
+                public float ComputeImpulseDenominator(Vector3 pos, Vector3 normal)
+                {
+                        Vector3 r0 = pos - CenterOfMassPosition;
+                        Vector3 c0 = Vector3.Cross(r0, normal);
+                        Vector3 vec = Vector3.Cross(Vector3.TransformNormal(c0, InvInertiaTensorWorld), r0);
+
+                        return _inverseMass + Vector3.Dot(normal, vec);
+
+                }
+
+                public float ComputeAngularImpulseDenominator(Vector3 axis)
+                {
+                        Vector3 vec = Vector3.TransformNormal(axis, InvInertiaTensorWorld);
+                        return Vector3.Dot(axis, vec);
+                }
+
+                public void UpdateDeactivation(float timeStep)
+                {
+                        if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.DisableDeactivation))
+                                return;
+
+                        if ((LinearVelocity.LengthSquared() < LinearSleepingThreshold * LinearSleepingThreshold) &&
+                                (AngularVelocity.LengthSquared() < AngularSleepingThreshold * AngularSleepingThreshold))
+                        {
+                                DeactivationTime += timeStep;
+                        }
+                        else
+                        {
+                                DeactivationTime = 0;
+                                ActivationState = ActivationState.Nothing;
+                        }
+
+                }
+
+                public bool WantsSleeping()
+                {
+
+                        if (ActivationState == ActivationState.DisableDeactivation)
+                                return false;
+
+                        //disable deactivation
+                        if (DisableDeactivation || (RigidBodyDeactivationTime == 0))
+                                return false;
+
+                        if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.WantsDeactivation))
+                                return true;
+
+                        if (DeactivationTime > RigidBodyDeactivationTime)
+                        {
+                                return true;
+                        }
+                        return false;
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs
index aa91a03..3430a73 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs
@@ -1,211 +1,211 @@
-/*

-  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.Dynamics

-{

-        public class SimpleDynamicsWorld : DynamicsWorld

-        {

-                private IConstraintSolver _constraintSolver;

-                private bool _ownsConstraintSolver;

-                private Vector3 _gravity;

-                private IDebugDraw _debugDrawer;

-

-                /// <summary>

-                /// this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver

-                /// </summary>

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

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

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

-                public SimpleDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver)

-                        : base(dispatcher, pairCache)

-                {

-                        _constraintSolver = constraintSolver;

-                        _ownsConstraintSolver = false;

-                        _gravity = new Vector3(0, 0, -10);

-                }

-

-                public override Vector3 Gravity

-                {

-                        set

-                        {

-                                _gravity = value;

-                                for (int i = 0; i < CollisionObjects.Count; i++)

-                                {

-                                        CollisionObject colObj = CollisionObjects[i];

-                                        RigidBody body = RigidBody.Upcast(colObj);

-                                        if (body != null)

-                                        {

-                                                body.Gravity = value;

-                                        }

-                                }

-                        }

-                }

-

-                public override IConstraintSolver ConstraintSolver

-                {

-                        set

-                        {

-                                _ownsConstraintSolver = false;

-                                _constraintSolver = value;

-                        }

-                }

-

-                public override IDebugDraw DebugDrawer

-                {

-                        get

-                        {

-                                return _debugDrawer;

-                        }

-                        set

-                        {

-                                _debugDrawer = value;

-                        }

-                }

-

-                public override void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep)

-                {

-                        //apply gravity, predict motion

-                        PredictUnconstraintMotion(timeStep);

-

-                        DispatcherInfo dispatchInfo = new DispatcherInfo();

-                        dispatchInfo.TimeStep = timeStep;

-                        dispatchInfo.StepCount = 0;

-                        dispatchInfo.DebugDraw = DebugDrawer;

-                        //perform collision detection

-                        PerformDiscreteCollisionDetection();

-

-                        //solve contact constraints

-                        int numManifolds = Dispatcher.ManifoldCount;

-                        if (numManifolds != 0)

-                        {

-

-                                List<PersistentManifold> manifolds = (Dispatcher as CollisionDispatcher).Manifolds;

-                                //int numManifolds = m_dispatcher1.GetNumManifolds();

-                                ContactSolverInfo infoGlobal = new ContactSolverInfo();

-                                infoGlobal.TimeStep = timeStep;

-

-                                _constraintSolver.SolveGroup(new List<CollisionObject>(), manifolds, manifolds.Count, new List<TypedConstraint>(), infoGlobal, _debugDrawer);

-                        }

-                        //integrate transforms

-                        IntegrateTransforms(timeStep);

-

-                        UpdateAabbs();

-

-                        SynchronizeMotionStates();

-                }

-

-                public override void UpdateAabbs()

-                {

-                        for (int i = 0; i < CollisionObjects.Count; i++)

-                        {

-                                CollisionObject colObj = CollisionObjects[i];

-                                RigidBody body = RigidBody.Upcast(colObj);

-                                if (body != null)

-                                {

-                                        if (body.IsActive && (!body.IsStaticObject))

-                                        {

-                                                Vector3 minAabb, maxAabb;

-                                                colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb);

-                                                IBroadphase bp = Broadphase;

-                                                bp.SetAabb(body.Broadphase, minAabb, maxAabb);

-                                        }

-                                }

-                        }

-                }

-

-                public override void AddRigidBody(RigidBody body)

-                {

-                        body.Gravity = _gravity;

-

-                        if (body.CollisionShape != null)

-                        {

-                                AddCollisionObject(body);

-                        }

-                }

-

-                public override void RemoveRigidBody(RigidBody body)

-                {

-                        RemoveCollisionObject(body);

-                }

-

-                public void SynchronizeMotionStates()

-                {

-                        for (int i = 0; i < CollisionObjects.Count; i++)

-                        {

-                                CollisionObject colObj = CollisionObjects[i];

-                                RigidBody body = RigidBody.Upcast(colObj);

-                                if (body != null && body.MotionState != null)

-                                {

-                                        if (body.ActivationState != ActivationState.IslandSleeping)

-                                        {

-                                                body.MotionState.SetWorldTransform(body.WorldTransform);

-                                        }

-                                }

-                        }

-                }

-

-                protected void PredictUnconstraintMotion(float timeStep)

-                {

-                        for (int i = 0; i < CollisionObjects.Count; i++)

-                        {

-                                CollisionObject colObj = CollisionObjects[i];

-                                RigidBody body = RigidBody.Upcast(colObj);

-                                if (body != null)

-                                {

-                                        if (!body.IsStaticObject)

-                                        {

-                                                if (body.IsActive)

-                                                {

-                                                        body.ApplyForces(timeStep);

-                                                        body.IntegrateVelocities(timeStep);

-                                                        Matrix temp = body.InterpolationWorldTransform;

-                                                        body.PredictIntegratedTransform(timeStep, ref temp);

-                                                        body.InterpolationWorldTransform = temp;

-                                                }

-                                        }

-                                }

-                        }

-                }

-

-                protected void IntegrateTransforms(float timeStep)

-                {

-                        Matrix predictedTrans = Matrix.Identity;

-                        for (int i = 0; i < CollisionObjects.Count; i++)

-                        {

-                                CollisionObject colObj = CollisionObjects[i];

-                                RigidBody body = RigidBody.Upcast(colObj);

-                                if (body != null)

-                                {

-                                        if (body.IsActive && (!body.IsStaticObject))

-                                        {

-                                                body.PredictIntegratedTransform(timeStep, ref predictedTrans);

-                                                body.ProceedToTransform(predictedTrans);

-                                        }

-                                }

-                        }

-                }

-        }

-}

+/*
+  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.Dynamics
+{
+        public class SimpleDynamicsWorld : DynamicsWorld
+        {
+                private IConstraintSolver _constraintSolver;
+                private bool _ownsConstraintSolver;
+                private Vector3 _gravity;
+                private IDebugDraw _debugDrawer;
+
+                /// <summary>
+                /// this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver
+                /// </summary>
+                /// <param name="dispatcher"></param>
+                /// <param name="pairCache"></param>
+                /// <param name="constraintSolver"></param>
+                public SimpleDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver)
+                        : base(dispatcher, pairCache)
+                {
+                        _constraintSolver = constraintSolver;
+                        _ownsConstraintSolver = false;
+                        _gravity = new Vector3(0, 0, -10);
+                }
+
+                public override Vector3 Gravity
+                {
+                        set
+                        {
+                                _gravity = value;
+                                for (int i = 0; i < CollisionObjects.Count; i++)
+                                {
+                                        CollisionObject colObj = CollisionObjects[i];
+                                        RigidBody body = RigidBody.Upcast(colObj);
+                                        if (body != null)
+                                        {
+                                                body.Gravity = value;
+                                        }
+                                }
+                        }
+                }
+
+                public override IConstraintSolver ConstraintSolver
+                {
+                        set
+                        {
+                                _ownsConstraintSolver = false;
+                                _constraintSolver = value;
+                        }
+                }
+
+                public override IDebugDraw DebugDrawer
+                {
+                        get
+                        {
+                                return _debugDrawer;
+                        }
+                        set
+                        {
+                                _debugDrawer = value;
+                        }
+                }
+
+                public override void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep)
+                {
+                        //apply gravity, predict motion
+                        PredictUnconstraintMotion(timeStep);
+
+                        DispatcherInfo dispatchInfo = new DispatcherInfo();
+                        dispatchInfo.TimeStep = timeStep;
+                        dispatchInfo.StepCount = 0;
+                        dispatchInfo.DebugDraw = DebugDrawer;
+                        //perform collision detection
+                        PerformDiscreteCollisionDetection();
+
+                        //solve contact constraints
+                        int numManifolds = Dispatcher.ManifoldCount;
+                        if (numManifolds != 0)
+                        {
+
+                                List<PersistentManifold> manifolds = (Dispatcher as CollisionDispatcher).Manifolds;
+                                //int numManifolds = m_dispatcher1.GetNumManifolds();
+                                ContactSolverInfo infoGlobal = new ContactSolverInfo();
+                                infoGlobal.TimeStep = timeStep;
+
+                                _constraintSolver.SolveGroup(new List<CollisionObject>(), manifolds, manifolds.Count, new List<TypedConstraint>(), infoGlobal, _debugDrawer);
+                        }
+                        //integrate transforms
+                        IntegrateTransforms(timeStep);
+
+                        UpdateAabbs();
+
+                        SynchronizeMotionStates();
+                }
+
+                public override void UpdateAabbs()
+                {
+                        for (int i = 0; i < CollisionObjects.Count; i++)
+                        {
+                                CollisionObject colObj = CollisionObjects[i];
+                                RigidBody body = RigidBody.Upcast(colObj);
+                                if (body != null)
+                                {
+                                        if (body.IsActive && (!body.IsStaticObject))
+                                        {
+                                                Vector3 minAabb, maxAabb;
+                                                colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb);
+                                                IBroadphase bp = Broadphase;
+                                                bp.SetAabb(body.Broadphase, minAabb, maxAabb);
+                                        }
+                                }
+                        }
+                }
+
+                public override void AddRigidBody(RigidBody body)
+                {
+                        body.Gravity = _gravity;
+
+                        if (body.CollisionShape != null)
+                        {
+                                AddCollisionObject(body);
+                        }
+                }
+
+                public override void RemoveRigidBody(RigidBody body)
+                {
+                        RemoveCollisionObject(body);
+                }
+
+                public void SynchronizeMotionStates()
+                {
+                        for (int i = 0; i < CollisionObjects.Count; i++)
+                        {
+                                CollisionObject colObj = CollisionObjects[i];
+                                RigidBody body = RigidBody.Upcast(colObj);
+                                if (body != null && body.MotionState != null)
+                                {
+                                        if (body.ActivationState != ActivationState.IslandSleeping)
+                                        {
+                                                body.MotionState.SetWorldTransform(body.WorldTransform);
+                                        }
+                                }
+                        }
+                }
+
+                protected void PredictUnconstraintMotion(float timeStep)
+                {
+                        for (int i = 0; i < CollisionObjects.Count; i++)
+                        {
+                                CollisionObject colObj = CollisionObjects[i];
+                                RigidBody body = RigidBody.Upcast(colObj);
+                                if (body != null)
+                                {
+                                        if (!body.IsStaticObject)
+                                        {
+                                                if (body.IsActive)
+                                                {
+                                                        body.ApplyForces(timeStep);
+                                                        body.IntegrateVelocities(timeStep);
+                                                        Matrix temp = body.InterpolationWorldTransform;
+                                                        body.PredictIntegratedTransform(timeStep, ref temp);
+                                                        body.InterpolationWorldTransform = temp;
+                                                }
+                                        }
+                                }
+                        }
+                }
+
+                protected void IntegrateTransforms(float timeStep)
+                {
+                        Matrix predictedTrans = Matrix.Identity;
+                        for (int i = 0; i < CollisionObjects.Count; i++)
+                        {
+                                CollisionObject colObj = CollisionObjects[i];
+                                RigidBody body = RigidBody.Upcast(colObj);
+                                if (body != null)
+                                {
+                                        if (body.IsActive && (!body.IsStaticObject))
+                                        {
+                                                body.PredictIntegratedTransform(timeStep, ref predictedTrans);
+                                                body.ProceedToTransform(predictedTrans);
+                                        }
+                                }
+                        }
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs
index 90db9ee..4e0e64b 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs
@@ -1,94 +1,94 @@
-/*

-  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.Dynamics

-{

-        public class RaycastVehicle : TypedConstraint

-        {

-                public override void BuildJacobian()

-                {

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

-                }

-

-                public override void SolveConstraint(float timeStep)

-                {

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

-                }

-

-                public int getNumWheels()

-                {

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

-                }

-

-                public WheelInfo getWheelInfo(int v)

-                {

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

-                }

-

-                public void updateWheelTransform(int v, bool p)

-                {

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

-                }

-

-                public int getRightAxis()

-                {

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

-                }

-

-                public void updateVehicle(float timeStep)

-                {

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

-                }

-        }

-

-        public class DefaultVehicleRaycaster : IVehicleRaycaster

-        {

-                DynamicsWorld _dynamicsWorld;

-

-                public DefaultVehicleRaycaster(DynamicsWorld world)

-                {

-                        _dynamicsWorld = world;

-                }

-

-                public object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result)

-                {

-                        CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(from, to);

-                        _dynamicsWorld.RayTest(from, to, rayCallback);

-

-                        result = new VehicleRaycasterResult();

-

-                        if (!rayCallback.HasHit) return 0;

-                        RigidBody body = RigidBody.Upcast(rayCallback.CollisionObject);

-                        if (body == null) return 0;

-

-                        result.HitPointInWorld = rayCallback.HitPointWorld;

-                        result.HitNormalInWorld = rayCallback.HitNormalWorld;

-                        result.HitNormalInWorld.Normalize();

-                        result.DistFraction = rayCallback.ClosestHitFraction;

-                        return body;

-                }

-        }

+/*
+  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.Dynamics
+{
+        public class RaycastVehicle : TypedConstraint
+        {
+                public override void BuildJacobian()
+                {
+                        throw new Exception("The method or operation is not implemented.");
+                }
+
+                public override void SolveConstraint(float timeStep)
+                {
+                        throw new Exception("The method or operation is not implemented.");
+                }
+
+                public int getNumWheels()
+                {
+                        throw new Exception("The method or operation is not implemented.");
+                }
+
+                public WheelInfo getWheelInfo(int v)
+                {
+                        throw new Exception("The method or operation is not implemented.");
+                }
+
+                public void updateWheelTransform(int v, bool p)
+                {
+                        throw new Exception("The method or operation is not implemented.");
+                }
+
+                public int getRightAxis()
+                {
+                        throw new Exception("The method or operation is not implemented.");
+                }
+
+                public void updateVehicle(float timeStep)
+                {
+                        throw new Exception("The method or operation is not implemented.");
+                }
+        }
+
+        public class DefaultVehicleRaycaster : IVehicleRaycaster
+        {
+                DynamicsWorld _dynamicsWorld;
+
+                public DefaultVehicleRaycaster(DynamicsWorld world)
+                {
+                        _dynamicsWorld = world;
+                }
+
+                public object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result)
+                {
+                        CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(from, to);
+                        _dynamicsWorld.RayTest(from, to, rayCallback);
+
+                        result = new VehicleRaycasterResult();
+
+                        if (!rayCallback.HasHit) return 0;
+                        RigidBody body = RigidBody.Upcast(rayCallback.CollisionObject);
+                        if (body == null) return 0;
+
+                        result.HitPointInWorld = rayCallback.HitPointWorld;
+                        result.HitNormalInWorld = rayCallback.HitNormalWorld;
+                        result.HitNormalInWorld.Normalize();
+                        result.DistFraction = rayCallback.ClosestHitFraction;
+                        return body;
+                }
+        }
 }
\ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/VehicleRaycaster.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/VehicleRaycaster.cs
index 86829e3..14c13c6 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/VehicleRaycaster.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/VehicleRaycaster.cs
@@ -1,49 +1,49 @@
-/*

-  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.Dynamics

-{

-        public interface IVehicleRaycaster

-        {

-                object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result);

-        }

-

-        public class VehicleRaycasterResult

-        {

-                private Single _distFraction;

-                private Vector3 _hitNormalInWorld;

-                private Vector3 _hitPointInWorld;

-

-                public VehicleRaycasterResult()

-                {

-                        _distFraction = -1;

-                }

-

-                public float DistFraction { get { return _distFraction; } set { _distFraction = value; } }

-                public Vector3 HitNormalInWorld { get { return _hitNormalInWorld; } set { _hitNormalInWorld = value; } }

-                public Vector3 HitPointInWorld { get { return _hitPointInWorld; } set { _hitPointInWorld = 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.Dynamics
+{
+        public interface IVehicleRaycaster
+        {
+                object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result);
+        }
+
+        public class VehicleRaycasterResult
+        {
+                private Single _distFraction;
+                private Vector3 _hitNormalInWorld;
+                private Vector3 _hitPointInWorld;
+
+                public VehicleRaycasterResult()
+                {
+                        _distFraction = -1;
+                }
+
+                public float DistFraction { get { return _distFraction; } set { _distFraction = value; } }
+                public Vector3 HitNormalInWorld { get { return _hitNormalInWorld; } set { _hitNormalInWorld = value; } }
+                public Vector3 HitPointInWorld { get { return _hitPointInWorld; } set { _hitPointInWorld = value; } }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs
index d4a94f3..516e455 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs
@@ -1,529 +1,529 @@
-/*

-  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.Dynamics

-{

-        public struct WheelInfoConstructionInfo

-        {

-                private Vector3 _chassicConnectionCS;

-                private Vector3 _wheelDirectionCS;

-                private Vector3 _wheelAxisCS;

-

-                private Single _suspensionRestLength;

-                private Single _maxSuspensionTravelCm;

-                private Single _wheelRadius;

-                private Single _suspensionStiffness;

-                private Single _wheelsDampingCompression;

-                private Single _wheelsDampingRelaxation;

-                private Single _frictionSlip;

-

-                private Boolean _isFrontWheel;

-

-                #region Basic Properties

-                public Vector3 ChassicConnectionCS

-                {

-                        get { return _chassicConnectionCS; }

-                        set { _chassicConnectionCS = value; }

-                }

-

-                public Vector3 WheelDirectionCS

-                {

-                        get { return _wheelDirectionCS; }

-                        set { _wheelDirectionCS = value; }

-                }

-

-                public Vector3 WheelAxleCS

-                {

-                        get { return _wheelAxisCS; }

-                        set { _wheelAxisCS = value; }

-                }

-

-

-                public Single SuspensionRestLength

-                {

-                        get { return _suspensionRestLength; }

-                        set { _suspensionRestLength = value; }

-                }

-

-                public Single MaxSuspensionTravelCm

-                {

-                        get { return _maxSuspensionTravelCm; }

-                        set { _maxSuspensionTravelCm = value; }

-                }

-

-                public Single WheelRadius

-                {

-                        get { return _wheelRadius; }

-                        set { _wheelRadius = value; }

-                }

-

-

-                public Single SuspensionStiffness

-                {

-                        get { return _suspensionStiffness; }

-                        set { _suspensionStiffness = value; }

-                }

-

-                public Single WheelsDampingCompression

-                {

-                        get { return _wheelsDampingCompression; }

-                        set { _wheelsDampingCompression = value; }

-                }

-

-                public Single WheelsDampingRelaxation

-                {

-                        get { return _wheelsDampingRelaxation; }

-                        set { _wheelsDampingRelaxation = value; }

-                }

-

-                public Single FrictionSlip

-                {

-                        get { return _frictionSlip; }

-                        set { _frictionSlip = value; }

-                }

-

-

-                public Boolean IsFrontWheel

-                {

-                        get { return _isFrontWheel; }

-                        set { _isFrontWheel = value; }

-                }

-                #endregion

-        }

-

-        public struct RaycastInfo

-        {

-                private Vector3 _contractNormalWS;

-                private Vector3 _contractPointWS;

-

-                private Vector3 _hardPointWS;

-                private Vector3 _wheelDirectionWS;

-                private Vector3 _wheelAxleWS;

-

-                private Single _suspensionLength;

-                private Boolean _isInContract;

-

-                #region Basic Properties

-                public Single SuspensionLength

-                {

-                        get { return _suspensionLength; }

-                        set { _suspensionLength = value; }

-                }

-

-                public Boolean IsInContact

-                {

-                        get { return _isInContract; }

-                        set { _isInContract = value; }

-                }

-

-                public Vector3 ContactNormalWS

-                {

-                        get { return _contractNormalWS; }

-                        set { _contractNormalWS = value; }

-                }

-

-                public Vector3 ContactPointWS

-                {

-                        get { return _contractPointWS; }

-                        set { _contractPointWS = value; }

-                }

-

-                public Vector3 HardPointWS

-                {

-                        get { return _hardPointWS; }

-                        set { _hardPointWS = value; }

-                }

-

-                public Vector3 WheelDirectionWS

-                {

-                        get { return _wheelDirectionWS; }

-                        set { _wheelDirectionWS = value; }

-                }

-

-                public Vector3 WheelAxleWS

-                {

-                        get { return _wheelAxleWS; }

-                        set { _wheelAxleWS = value; }

-                }

-                #endregion

-        }

-

-        public struct WheelInfo

-        {

-                private RaycastInfo _raycastInfo;

-

-                private Matrix _worldTransform;

-

-                private Vector3 _chassicConnectionPointCS;

-                private Vector3 _wheelDirectionCS;

-                private Vector3 _wheelAxleCS;

-

-                private Single _suspensionRestLength;

-                private Single _maxSuspensionTravelCm;

-

-                private Single _wheelsRadius;

-                private Single _rollInfluence;

-                private Single _suspensionStiffness;

-                private Single _wheelsDampingCompression;

-                private Single _wheelsDampingRelaxation;

-                private Single _frictionSlip;

-                private Single _steering;

-                private Single _rotation;

-                private Single _deltaRotation;

-

-                private Single _engineForce;

-                private Single _brake;

-                private Boolean _isFrontWheel;

-

-

-                private Single _clippedInvContactDotSuspension;

-                private Single _skidInfo;

-                private Single _wheelsSuspensionForce;

-                private Single _suspensionRelativeVelocity;

-                //can be used to store pointer to sync transforms...

-                private object _clientInfo;

-

-                #region Constructor

-                public WheelInfo(WheelInfoConstructionInfo constructionInfo)

-                {

-                        _suspensionRestLength = constructionInfo.SuspensionRestLength;

-                        _maxSuspensionTravelCm = constructionInfo.MaxSuspensionTravelCm;

-

-                        _wheelsRadius = constructionInfo.WheelRadius;

-                        _wheelsDampingCompression = constructionInfo.WheelsDampingCompression;

-                        _wheelsDampingRelaxation = constructionInfo.WheelsDampingRelaxation;

-                        _wheelDirectionCS = constructionInfo.WheelDirectionCS;

-

-                        _suspensionStiffness = constructionInfo.SuspensionStiffness;

-                        _chassicConnectionPointCS = constructionInfo.ChassicConnectionCS;

-

-                        _wheelAxleCS = constructionInfo.WheelAxleCS;

-                        _frictionSlip = constructionInfo.FrictionSlip;

-

-                        _clippedInvContactDotSuspension = 0;

-                        _suspensionRelativeVelocity = 0;

-                        _wheelsSuspensionForce = 0;

-                        _skidInfo = 0;

-

-                        _steering = 0;

-                        _engineForce = 0;

-                        _rotation = 0;

-                        _rotation = 0;

-                        _deltaRotation = 0;

-                        _brake = 0;

-                        _rollInfluence = 0.1f;

-                        _brake = 0;

-                        _rollInfluence = 0.1f;

-

-                        _isFrontWheel = constructionInfo.IsFrontWheel;

-

-                        _raycastInfo = default(RaycastInfo);

-                        _worldTransform = default(Matrix);

-                        _clientInfo = null;

-                }

-                #endregion

-

-                #region BasicProperties

-                public object ClientInfo { get { return _clientInfo; } set { _clientInfo = value; } }

-

-                public RaycastInfo RaycastInfo

-                {

-                        get { return _raycastInfo; }

-                        set { _raycastInfo = value; }

-                }

-

-                public Matrix WorldTransform

-                {

-                        get { return _worldTransform; }

-                        set { _worldTransform = value; }

-                }

-

-                public Vector3 ChassicConnectionPointCS

-                {

-                        get { return _chassicConnectionPointCS; }

-                        set { _chassicConnectionPointCS = value; }

-                }

-                public Vector3 WheelDirectionCS

-                {

-                        get { return _wheelDirectionCS; }

-                        set { _wheelDirectionCS = value; }

-                }

-                public Vector3 WheelAxleCS

-                {

-                        get { return _wheelAxleCS; }

-                        set { _wheelAxleCS = value; }

-                }

-

-                public Single SuspensionRestLength

-                {

-                        get { return _suspensionRestLength; }

-                        set { _suspensionRestLength = value; }

-                }

-

-

-                public Single MaxSuspensionTravelCm

-                {

-                        get { return _maxSuspensionTravelCm; }

-                        set { _maxSuspensionTravelCm = value; }

-                }

-

-                public Single WheelsRadius

-                {

-                        get { return _wheelsRadius; }

-                        set { _wheelsRadius = value; }

-                }

-

-                public Single SuspensionStiffness

-                {

-                        get { return _suspensionStiffness; }

-                        set { _suspensionStiffness = value; }

-                }

-

-                public Single WheelsDampingCompression

-                {

-                        get { return _wheelsDampingCompression; }

-                        set { _wheelsDampingCompression = value; }

-                }

-

-                public Single WheelsDampingRelaxation

-                {

-                        get { return _wheelsDampingRelaxation; }

-                        set { _wheelsDampingRelaxation = value; }

-                }

-

-                public Single FrictionSlip

-                {

-                        get { return _frictionSlip; }

-                        set { _frictionSlip = value; }

-                }

-

-                public Single Steering

-                {

-                        get { return _steering; }

-                        set { _steering = value; }

-                }

-

-                public Single Rotation

-                {

-                        get { return _rotation; }

-                        set { _rotation = value; }

-                }

-

-                public Single DeltaRotation

-                {

-                        get { return _deltaRotation; }

-                        set { _deltaRotation = value; }

-                }

-

-                public Single RollInfluence

-                {

-                        get { return _rollInfluence; }

-                        set { _rollInfluence = value; }

-                }

-

-                public Single EngineForce

-                {

-                        get { return _engineForce; }

-                        set { _engineForce = value; }

-                }

-

-                public Single Brake

-                {

-                        get { return _brake; }

-                        set { _brake = value; }

-                }

-

-                public Boolean IsFrontWheel

-                {

-                        get { return _isFrontWheel; }

-                        set { _isFrontWheel = value; }

-                }

-

-                public Single ClippedInvContactDotSuspension

-                {

-                        get { return _clippedInvContactDotSuspension; }

-                        set { _clippedInvContactDotSuspension = value; }

-                }

-

-                public Single SuspensionRelativeVelocity

-                {

-                        get { return _suspensionRelativeVelocity; }

-                        set { _suspensionRelativeVelocity = value; }

-                }

-

-                public Single WheelsSuspensionForce

-                {

-                        get { return _wheelsSuspensionForce; }

-                        set { _wheelsSuspensionForce = value; }

-                }

-

-                public Single SkidInfo

-                {

-                        get { return _skidInfo; }

-                        set { _skidInfo = value; }

-                }

-                #endregion

-

-                /// <summary>

-                /// 

-                /// </summary>

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

-                /// <param name="paramRaycastInfo">Not used!</param>

-                public void UpdateWheel(RigidBody chassis, RaycastInfo paramRaycastInfo)

-                {

-                        if (_raycastInfo.IsInContact)

-                        {

-                                float project = Vector3.Dot(_raycastInfo.ContactNormalWS, _raycastInfo.WheelDirectionWS);

-

-                                Vector3 chassisVelocityAtContactPoint = new Vector3();

-                                Vector3 relpos = _raycastInfo.ContactPointWS - chassis.CenterOfMassPosition;

-                                chassisVelocityAtContactPoint = chassis.GetVelocityInLocalPoint(relpos);

-                                float projVel = Vector3.Dot(_raycastInfo.ContactNormalWS, chassisVelocityAtContactPoint);

-

-                                if (project >= -0.1f)

-                                {

-                                        _suspensionRelativeVelocity = 0;

-                                        _clippedInvContactDotSuspension = 1.0f / 0.1f;

-                                }

-                                else

-                                {

-                                        float inv = -1 / project;

-                                        _suspensionRelativeVelocity = projVel * inv;

-                                        _clippedInvContactDotSuspension = inv;

-                                }

-                        }

-                        else

-                        {

-                                _raycastInfo.SuspensionLength = _suspensionRestLength;

-                                _suspensionRelativeVelocity = 0.0f;

-                                _raycastInfo.ContactNormalWS = -_raycastInfo.WheelDirectionWS;

-                                _clippedInvContactDotSuspension = 1.0f;

-                        }

-                }

-

-                //        if (m_raycastInfo.m_isInContact)

-

-                //{

-                //    btScalar  project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS );

-                //    btVector3  chassis_velocity_at_contactPoint;

-                //    btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition();

-                //    chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos );

-                //    btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );

-                //    if ( project >= -0.1f)

-                //    {

-                //        m_suspensionRelativeVelocity = 0.0f;

-                //        m_clippedInvContactDotSuspension = 1.0f / 0.1f;

-                //    }

-                //    else

-                //    {

-                //        btScalar inv = -1.f / project;

-                //        m_suspensionRelativeVelocity = projVel * inv;

-                //        m_clippedInvContactDotSuspension = inv;

-                //    }

-

-                //}

-

-                //else  // Not in contact : position wheel in a nice (rest length) position

-                //{

-                //    m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength();

-                //    m_suspensionRelativeVelocity = 0.0f;

-                //    m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS;

-                //    m_clippedInvContactDotSuspension = 1.0f;

-                //}

-        };

-

-        //btScalar      m_clippedInvContactDotSuspension;

-        //btScalar      m_suspensionRelativeVelocity;

-        //btScalar      m_wheelsSuspensionForce;

-        //btScalar      m_skidInfo;

-

-        //void*         m_clientInfo;//can be used to store pointer to sync transforms...

-

-        //btWheelInfo(btWheelInfoConstructionInfo& ci)

-

-        //{

-

-        //    m_suspensionRestLength1 = ci.m_suspensionRestLength;

-        //    m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm;

-

-        //    m_wheelsRadius = ci.m_wheelRadius;

-        //    m_suspensionStiffness = ci.m_suspensionStiffness;

-        //    m_wheelsDampingCompression = ci.m_wheelsDampingCompression;

-        //    m_wheelsDampingRelaxation = ci.m_wheelsDampingRelaxation;

-        //    m_chassisConnectionPointCS = ci.m_chassisConnectionCS;

-        //    m_wheelDirectionCS = ci.m_wheelDirectionCS;

-        //    m_wheelAxleCS = ci.m_wheelAxleCS;

-        //    m_frictionSlip = ci.m_frictionSlip;

-        //    m_steering = 0.f;

-        //    m_engineForce = 0.f;

-        //    m_rotation = 0.f;

-        //    m_deltaRotation = 0.f;

-        //    m_brake = 0.f;

-        //    m_rollInfluence = 0.1f;

-        //    m_bIsFrontWheel = ci.m_bIsFrontWheel;

-

-        //}

-

-        //void  updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo);

-

-        //btScalar      m_clippedInvContactDotSuspension;

-        //btScalar      m_suspensionRelativeVelocity;

-        ////calculated by suspension

-        //btScalar      m_wheelsSuspensionForce;

-        //btScalar      m_skidInfo;

-

-        //};  

-

-        //struct RaycastInfo

-        //{

-        //    //set by raycaster

-        //    btVector3 m_contactNormalWS;//contactnormal

-        //    btVector3 m_contactPointWS;//raycast hitpoint

-        //    btScalar m_suspensionLength;

-        //    btVector3 m_hardPointWS;//raycast starting point

-        //    btVector3 m_wheelDirectionWS; //direction in worldspace

-        //    btVector3 m_wheelAxleWS; // axle in worldspace

-        //    bool m_isInContact;

-        //    void* m_groundObject; //could be general void* ptr

-        //};

-

-        //struct btWheelInfoConstructionInfo

-        //{

-        //    btVector3 m_chassisConnectionCS;

-        //    btVector3 m_wheelDirectionCS;

-        //    btVector3 m_wheelAxleCS;

-        //    btScalar m_suspensionRestLength;

-        //    btScalar m_maxSuspensionTravelCm;

-        //    btScalar m_wheelRadius;

-

-        //    float m_suspensionStiffness;

-        //    float m_wheelsDampingCompression;

-        //    float m_wheelsDampingRelaxation;

-        //    float m_frictionSlip;

-        //    bool m_bIsFrontWheel;

-

-        //};

-}

+/*
+  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.Dynamics
+{
+        public struct WheelInfoConstructionInfo
+        {
+                private Vector3 _chassicConnectionCS;
+                private Vector3 _wheelDirectionCS;
+                private Vector3 _wheelAxisCS;
+
+                private Single _suspensionRestLength;
+                private Single _maxSuspensionTravelCm;
+                private Single _wheelRadius;
+                private Single _suspensionStiffness;
+                private Single _wheelsDampingCompression;
+                private Single _wheelsDampingRelaxation;
+                private Single _frictionSlip;
+
+                private Boolean _isFrontWheel;
+
+                #region Basic Properties
+                public Vector3 ChassicConnectionCS
+                {
+                        get { return _chassicConnectionCS; }
+                        set { _chassicConnectionCS = value; }
+                }
+
+                public Vector3 WheelDirectionCS
+                {
+                        get { return _wheelDirectionCS; }
+                        set { _wheelDirectionCS = value; }
+                }
+
+                public Vector3 WheelAxleCS
+                {
+                        get { return _wheelAxisCS; }
+                        set { _wheelAxisCS = value; }
+                }
+
+
+                public Single SuspensionRestLength
+                {
+                        get { return _suspensionRestLength; }
+                        set { _suspensionRestLength = value; }
+                }
+
+                public Single MaxSuspensionTravelCm
+                {
+                        get { return _maxSuspensionTravelCm; }
+                        set { _maxSuspensionTravelCm = value; }
+                }
+
+                public Single WheelRadius
+                {
+                        get { return _wheelRadius; }
+                        set { _wheelRadius = value; }
+                }
+
+
+                public Single SuspensionStiffness
+                {
+                        get { return _suspensionStiffness; }
+                        set { _suspensionStiffness = value; }
+                }
+
+                public Single WheelsDampingCompression
+                {
+                        get { return _wheelsDampingCompression; }
+                        set { _wheelsDampingCompression = value; }
+                }
+
+                public Single WheelsDampingRelaxation
+                {
+                        get { return _wheelsDampingRelaxation; }
+                        set { _wheelsDampingRelaxation = value; }
+                }
+
+                public Single FrictionSlip
+                {
+                        get { return _frictionSlip; }
+                        set { _frictionSlip = value; }
+                }
+
+
+                public Boolean IsFrontWheel
+                {
+                        get { return _isFrontWheel; }
+                        set { _isFrontWheel = value; }
+                }
+                #endregion
+        }
+
+        public struct RaycastInfo
+        {
+                private Vector3 _contractNormalWS;
+                private Vector3 _contractPointWS;
+
+                private Vector3 _hardPointWS;
+                private Vector3 _wheelDirectionWS;
+                private Vector3 _wheelAxleWS;
+
+                private Single _suspensionLength;
+                private Boolean _isInContract;
+
+                #region Basic Properties
+                public Single SuspensionLength
+                {
+                        get { return _suspensionLength; }
+                        set { _suspensionLength = value; }
+                }
+
+                public Boolean IsInContact
+                {
+                        get { return _isInContract; }
+                        set { _isInContract = value; }
+                }
+
+                public Vector3 ContactNormalWS
+                {
+                        get { return _contractNormalWS; }
+                        set { _contractNormalWS = value; }
+                }
+
+                public Vector3 ContactPointWS
+                {
+                        get { return _contractPointWS; }
+                        set { _contractPointWS = value; }
+                }
+
+                public Vector3 HardPointWS
+                {
+                        get { return _hardPointWS; }
+                        set { _hardPointWS = value; }
+                }
+
+                public Vector3 WheelDirectionWS
+                {
+                        get { return _wheelDirectionWS; }
+                        set { _wheelDirectionWS = value; }
+                }
+
+                public Vector3 WheelAxleWS
+                {
+                        get { return _wheelAxleWS; }
+                        set { _wheelAxleWS = value; }
+                }
+                #endregion
+        }
+
+        public struct WheelInfo
+        {
+                private RaycastInfo _raycastInfo;
+
+                private Matrix _worldTransform;
+
+                private Vector3 _chassicConnectionPointCS;
+                private Vector3 _wheelDirectionCS;
+                private Vector3 _wheelAxleCS;
+
+                private Single _suspensionRestLength;
+                private Single _maxSuspensionTravelCm;
+
+                private Single _wheelsRadius;
+                private Single _rollInfluence;
+                private Single _suspensionStiffness;
+                private Single _wheelsDampingCompression;
+                private Single _wheelsDampingRelaxation;
+                private Single _frictionSlip;
+                private Single _steering;
+                private Single _rotation;
+                private Single _deltaRotation;
+
+                private Single _engineForce;
+                private Single _brake;
+                private Boolean _isFrontWheel;
+
+
+                private Single _clippedInvContactDotSuspension;
+                private Single _skidInfo;
+                private Single _wheelsSuspensionForce;
+                private Single _suspensionRelativeVelocity;
+                //can be used to store pointer to sync transforms...
+                private object _clientInfo;
+
+                #region Constructor
+                public WheelInfo(WheelInfoConstructionInfo constructionInfo)
+                {
+                        _suspensionRestLength = constructionInfo.SuspensionRestLength;
+                        _maxSuspensionTravelCm = constructionInfo.MaxSuspensionTravelCm;
+
+                        _wheelsRadius = constructionInfo.WheelRadius;
+                        _wheelsDampingCompression = constructionInfo.WheelsDampingCompression;
+                        _wheelsDampingRelaxation = constructionInfo.WheelsDampingRelaxation;
+                        _wheelDirectionCS = constructionInfo.WheelDirectionCS;
+
+                        _suspensionStiffness = constructionInfo.SuspensionStiffness;
+                        _chassicConnectionPointCS = constructionInfo.ChassicConnectionCS;
+
+                        _wheelAxleCS = constructionInfo.WheelAxleCS;
+                        _frictionSlip = constructionInfo.FrictionSlip;
+
+                        _clippedInvContactDotSuspension = 0;
+                        _suspensionRelativeVelocity = 0;
+                        _wheelsSuspensionForce = 0;
+                        _skidInfo = 0;
+
+                        _steering = 0;
+                        _engineForce = 0;
+                        _rotation = 0;
+                        _rotation = 0;
+                        _deltaRotation = 0;
+                        _brake = 0;
+                        _rollInfluence = 0.1f;
+                        _brake = 0;
+                        _rollInfluence = 0.1f;
+
+                        _isFrontWheel = constructionInfo.IsFrontWheel;
+
+                        _raycastInfo = default(RaycastInfo);
+                        _worldTransform = default(Matrix);
+                        _clientInfo = null;
+                }
+                #endregion
+
+                #region BasicProperties
+                public object ClientInfo { get { return _clientInfo; } set { _clientInfo = value; } }
+
+                public RaycastInfo RaycastInfo
+                {
+                        get { return _raycastInfo; }
+                        set { _raycastInfo = value; }
+                }
+
+                public Matrix WorldTransform
+                {
+                        get { return _worldTransform; }
+                        set { _worldTransform = value; }
+                }
+
+                public Vector3 ChassicConnectionPointCS
+                {
+                        get { return _chassicConnectionPointCS; }
+                        set { _chassicConnectionPointCS = value; }
+                }
+                public Vector3 WheelDirectionCS
+                {
+                        get { return _wheelDirectionCS; }
+                        set { _wheelDirectionCS = value; }
+                }
+                public Vector3 WheelAxleCS
+                {
+                        get { return _wheelAxleCS; }
+                        set { _wheelAxleCS = value; }
+                }
+
+                public Single SuspensionRestLength
+                {
+                        get { return _suspensionRestLength; }
+                        set { _suspensionRestLength = value; }
+                }
+
+
+                public Single MaxSuspensionTravelCm
+                {
+                        get { return _maxSuspensionTravelCm; }
+                        set { _maxSuspensionTravelCm = value; }
+                }
+
+                public Single WheelsRadius
+                {
+                        get { return _wheelsRadius; }
+                        set { _wheelsRadius = value; }
+                }
+
+                public Single SuspensionStiffness
+                {
+                        get { return _suspensionStiffness; }
+                        set { _suspensionStiffness = value; }
+                }
+
+                public Single WheelsDampingCompression
+                {
+                        get { return _wheelsDampingCompression; }
+                        set { _wheelsDampingCompression = value; }
+                }
+
+                public Single WheelsDampingRelaxation
+                {
+                        get { return _wheelsDampingRelaxation; }
+                        set { _wheelsDampingRelaxation = value; }
+                }
+
+                public Single FrictionSlip
+                {
+                        get { return _frictionSlip; }
+                        set { _frictionSlip = value; }
+                }
+
+                public Single Steering
+                {
+                        get { return _steering; }
+                        set { _steering = value; }
+                }
+
+                public Single Rotation
+                {
+                        get { return _rotation; }
+                        set { _rotation = value; }
+                }
+
+                public Single DeltaRotation
+                {
+                        get { return _deltaRotation; }
+                        set { _deltaRotation = value; }
+                }
+
+                public Single RollInfluence
+                {
+                        get { return _rollInfluence; }
+                        set { _rollInfluence = value; }
+                }
+
+                public Single EngineForce
+                {
+                        get { return _engineForce; }
+                        set { _engineForce = value; }
+                }
+
+                public Single Brake
+                {
+                        get { return _brake; }
+                        set { _brake = value; }
+                }
+
+                public Boolean IsFrontWheel
+                {
+                        get { return _isFrontWheel; }
+                        set { _isFrontWheel = value; }
+                }
+
+                public Single ClippedInvContactDotSuspension
+                {
+                        get { return _clippedInvContactDotSuspension; }
+                        set { _clippedInvContactDotSuspension = value; }
+                }
+
+                public Single SuspensionRelativeVelocity
+                {
+                        get { return _suspensionRelativeVelocity; }
+                        set { _suspensionRelativeVelocity = value; }
+                }
+
+                public Single WheelsSuspensionForce
+                {
+                        get { return _wheelsSuspensionForce; }
+                        set { _wheelsSuspensionForce = value; }
+                }
+
+                public Single SkidInfo
+                {
+                        get { return _skidInfo; }
+                        set { _skidInfo = value; }
+                }
+                #endregion
+
+                /// <summary>
+                /// 
+                /// </summary>
+                /// <param name="chassis"></param>
+                /// <param name="paramRaycastInfo">Not used!</param>
+                public void UpdateWheel(RigidBody chassis, RaycastInfo paramRaycastInfo)
+                {
+                        if (_raycastInfo.IsInContact)
+                        {
+                                float project = Vector3.Dot(_raycastInfo.ContactNormalWS, _raycastInfo.WheelDirectionWS);
+
+                                Vector3 chassisVelocityAtContactPoint = new Vector3();
+                                Vector3 relpos = _raycastInfo.ContactPointWS - chassis.CenterOfMassPosition;
+                                chassisVelocityAtContactPoint = chassis.GetVelocityInLocalPoint(relpos);
+                                float projVel = Vector3.Dot(_raycastInfo.ContactNormalWS, chassisVelocityAtContactPoint);
+
+                                if (project >= -0.1f)
+                                {
+                                        _suspensionRelativeVelocity = 0;
+                                        _clippedInvContactDotSuspension = 1.0f / 0.1f;
+                                }
+                                else
+                                {
+                                        float inv = -1 / project;
+                                        _suspensionRelativeVelocity = projVel * inv;
+                                        _clippedInvContactDotSuspension = inv;
+                                }
+                        }
+                        else
+                        {
+                                _raycastInfo.SuspensionLength = _suspensionRestLength;
+                                _suspensionRelativeVelocity = 0.0f;
+                                _raycastInfo.ContactNormalWS = -_raycastInfo.WheelDirectionWS;
+                                _clippedInvContactDotSuspension = 1.0f;
+                        }
+                }
+
+                //        if (m_raycastInfo.m_isInContact)
+
+                //{
+                //    btScalar  project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS );
+                //    btVector3  chassis_velocity_at_contactPoint;
+                //    btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition();
+                //    chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos );
+                //    btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
+                //    if ( project >= -0.1f)
+                //    {
+                //        m_suspensionRelativeVelocity = 0.0f;
+                //        m_clippedInvContactDotSuspension = 1.0f / 0.1f;
+                //    }
+                //    else
+                //    {
+                //        btScalar inv = -1.f / project;
+                //        m_suspensionRelativeVelocity = projVel * inv;
+                //        m_clippedInvContactDotSuspension = inv;
+                //    }
+
+                //}
+
+                //else  // Not in contact : position wheel in a nice (rest length) position
+                //{
+                //    m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength();
+                //    m_suspensionRelativeVelocity = 0.0f;
+                //    m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS;
+                //    m_clippedInvContactDotSuspension = 1.0f;
+                //}
+        };
+
+        //btScalar      m_clippedInvContactDotSuspension;
+        //btScalar      m_suspensionRelativeVelocity;
+        //btScalar      m_wheelsSuspensionForce;
+        //btScalar      m_skidInfo;
+
+        //void*         m_clientInfo;//can be used to store pointer to sync transforms...
+
+        //btWheelInfo(btWheelInfoConstructionInfo& ci)
+
+        //{
+
+        //    m_suspensionRestLength1 = ci.m_suspensionRestLength;
+        //    m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm;
+
+        //    m_wheelsRadius = ci.m_wheelRadius;
+        //    m_suspensionStiffness = ci.m_suspensionStiffness;
+        //    m_wheelsDampingCompression = ci.m_wheelsDampingCompression;
+        //    m_wheelsDampingRelaxation = ci.m_wheelsDampingRelaxation;
+        //    m_chassisConnectionPointCS = ci.m_chassisConnectionCS;
+        //    m_wheelDirectionCS = ci.m_wheelDirectionCS;
+        //    m_wheelAxleCS = ci.m_wheelAxleCS;
+        //    m_frictionSlip = ci.m_frictionSlip;
+        //    m_steering = 0.f;
+        //    m_engineForce = 0.f;
+        //    m_rotation = 0.f;
+        //    m_deltaRotation = 0.f;
+        //    m_brake = 0.f;
+        //    m_rollInfluence = 0.1f;
+        //    m_bIsFrontWheel = ci.m_bIsFrontWheel;
+
+        //}
+
+        //void  updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo);
+
+        //btScalar      m_clippedInvContactDotSuspension;
+        //btScalar      m_suspensionRelativeVelocity;
+        ////calculated by suspension
+        //btScalar      m_wheelsSuspensionForce;
+        //btScalar      m_skidInfo;
+
+        //};  
+
+        //struct RaycastInfo
+        //{
+        //    //set by raycaster
+        //    btVector3 m_contactNormalWS;//contactnormal
+        //    btVector3 m_contactPointWS;//raycast hitpoint
+        //    btScalar m_suspensionLength;
+        //    btVector3 m_hardPointWS;//raycast starting point
+        //    btVector3 m_wheelDirectionWS; //direction in worldspace
+        //    btVector3 m_wheelAxleWS; // axle in worldspace
+        //    bool m_isInContact;
+        //    void* m_groundObject; //could be general void* ptr
+        //};
+
+        //struct btWheelInfoConstructionInfo
+        //{
+        //    btVector3 m_chassisConnectionCS;
+        //    btVector3 m_wheelDirectionCS;
+        //    btVector3 m_wheelAxleCS;
+        //    btScalar m_suspensionRestLength;
+        //    btScalar m_maxSuspensionTravelCm;
+        //    btScalar m_wheelRadius;
+
+        //    float m_suspensionStiffness;
+        //    float m_wheelsDampingCompression;
+        //    float m_wheelsDampingRelaxation;
+        //    float m_frictionSlip;
+        //    bool m_bIsFrontWheel;
+
+        //};
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs
index abd4885..3ae3ad6 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs
@@ -1,53 +1,53 @@
-/*

-  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 System.Runtime.Serialization;

-

-namespace XnaDevRu.BulletX

-{

-        public class BulletException : Exception

-        {

-                internal BulletException()

-                        : base("Bullet Physics Library has thrown an exception.")

-                {

-                }

-

-                internal BulletException(string message)

-                        : base(message)

-                {

-                }

-

-                internal BulletException(string message, Exception innerException)

-                        : base(message, innerException)

-                {

-                }

-

-#if !XBOX

-                internal BulletException(SerializationInfo info, StreamingContext context)

-                        : base(info, context)

-                {

-                }

-#endif

-        }

-}

+/*
+  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 System.Runtime.Serialization;
+
+namespace XnaDevRu.BulletX
+{
+        public class BulletException : Exception
+        {
+                internal BulletException()
+                        : base("Bullet Physics Library has thrown an exception.")
+                {
+                }
+
+                internal BulletException(string message)
+                        : base(message)
+                {
+                }
+
+                internal BulletException(string message, Exception innerException)
+                        : base(message, innerException)
+                {
+                }
+
+#if !XBOX
+                internal BulletException(SerializationInfo info, StreamingContext context)
+                        : base(info, context)
+                {
+                }
+#endif
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs b/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs
index ef5b5dc..5af979c 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs
@@ -1,60 +1,60 @@
-/*

-  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 enum DebugDrawModes

-        {

-                NoDebug = 0,

-                DrawWireframe = 1,

-                DrawAabb = 2,

-                DrawFeaturesText = 4,

-                DrawContactPoints = 8,

-                NoDeactivation = 16,

-                NoHelpText = 32,

-                DrawText = 64,

-                ProfileTimings = 128,

-                EnableSatComparison = 256,

-                DisableBulletLcp = 512,

-                EnableCcd = 1024,

-                MaxDebugDrawMode

-        }

-

-    public interface IDebugDraw

-    {

-        void DrawLine(Vector3 from, Vector3 to, Vector3 color);

-

-        void DrawContactPoint(

-            Vector3 pointOnB,

-            Vector3 normalOnB,

-            float distance,

-            int lifeTime,

-            Vector3 color

-        );

-

-                DebugDrawModes DebugMode { get; set; }

-    }

+/*
+  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 enum DebugDrawModes
+        {
+                NoDebug = 0,
+                DrawWireframe = 1,
+                DrawAabb = 2,
+                DrawFeaturesText = 4,
+                DrawContactPoints = 8,
+                NoDeactivation = 16,
+                NoHelpText = 32,
+                DrawText = 64,
+                ProfileTimings = 128,
+                EnableSatComparison = 256,
+                DisableBulletLcp = 512,
+                EnableCcd = 1024,
+                MaxDebugDrawMode
+        }
+
+    public interface IDebugDraw
+    {
+        void DrawLine(Vector3 from, Vector3 to, Vector3 color);
+
+        void DrawContactPoint(
+            Vector3 pointOnB,
+            Vector3 normalOnB,
+            float distance,
+            int lifeTime,
+            Vector3 color
+        );
+
+                DebugDrawModes DebugMode { get; set; }
+    }
 }
\ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs
index 2a45a20..73dc675 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs
@@ -1,66 +1,66 @@
-/*

-  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

-{

-        // DefaultMotionState provides a common implementation to synchronize world transforms with offsets

-        public class DefaultMotionState : MotionState

-        {

-                private Matrix _graphicsWorldTransform;

-                private Matrix _centerOfMassOffset;

-                private Matrix _startWorldTransform;

-                private object _userData;

-

-                public DefaultMotionState()

-                        : this(Matrix.Identity, Matrix.Identity) { }

-

-                public DefaultMotionState(Matrix startTransform, Matrix centerOfMassOffset)

-                {

-                        _graphicsWorldTransform = startTransform;

-                        _centerOfMassOffset = centerOfMassOffset;

-                        _startWorldTransform = startTransform;

-                }

-

-                public Matrix GraphicsWorldTransform { get { return _graphicsWorldTransform; } set { _graphicsWorldTransform = value; } }

-                public Matrix CenterOfMassOffset { get { return _centerOfMassOffset; } set { _centerOfMassOffset = value; } }

-                public Matrix StartWorldTransform { get { return _startWorldTransform; } set { _startWorldTransform = value; } }

-                public object UserData { get { return _userData; } set { _userData = value; } }

-

-                // synchronizes world transform from user to physics

-                public override void GetWorldTransform(out Matrix centerOfMassWorldTrans)

-                {

-            centerOfMassWorldTrans = MathHelper.InvertMatrix(_centerOfMassOffset) * _graphicsWorldTransform;

-                }

-

-                // synchronizes world transform from physics to user

-                // Bullet only calls the update of worldtransform for active objects

-                public override void SetWorldTransform(Matrix centerOfMassWorldTrans)

-                {

-                        _graphicsWorldTransform = MatrixOperations.Multiply(centerOfMassWorldTrans, _centerOfMassOffset);

-                        _graphicsWorldTransform.Translation = centerOfMassWorldTrans.Translation;

-                }

-        }

-}

+/*
+  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
+{
+        // DefaultMotionState provides a common implementation to synchronize world transforms with offsets
+        public class DefaultMotionState : MotionState
+        {
+                private Matrix _graphicsWorldTransform;
+                private Matrix _centerOfMassOffset;
+                private Matrix _startWorldTransform;
+                private object _userData;
+
+                public DefaultMotionState()
+                        : this(Matrix.Identity, Matrix.Identity) { }
+
+                public DefaultMotionState(Matrix startTransform, Matrix centerOfMassOffset)
+                {
+                        _graphicsWorldTransform = startTransform;
+                        _centerOfMassOffset = centerOfMassOffset;
+                        _startWorldTransform = startTransform;
+                }
+
+                public Matrix GraphicsWorldTransform { get { return _graphicsWorldTransform; } set { _graphicsWorldTransform = value; } }
+                public Matrix CenterOfMassOffset { get { return _centerOfMassOffset; } set { _centerOfMassOffset = value; } }
+                public Matrix StartWorldTransform { get { return _startWorldTransform; } set { _startWorldTransform = value; } }
+                public object UserData { get { return _userData; } set { _userData = value; } }
+
+                // synchronizes world transform from user to physics
+                public override void GetWorldTransform(out Matrix centerOfMassWorldTrans)
+                {
+            centerOfMassWorldTrans = MathHelper.InvertMatrix(_centerOfMassOffset) * _graphicsWorldTransform;
+                }
+
+                // synchronizes world transform from physics to user
+                // Bullet only calls the update of worldtransform for active objects
+                public override void SetWorldTransform(Matrix centerOfMassWorldTrans)
+                {
+                        _graphicsWorldTransform = MatrixOperations.Multiply(centerOfMassWorldTrans, _centerOfMassOffset);
+                        _graphicsWorldTransform.Translation = centerOfMassWorldTrans.Translation;
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs
index 2e3f258..15c7bb8 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs
@@ -1,581 +1,581 @@
-/*

-  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 static class MathHelper

-        {

-                internal const float Sqrt12 = 0.7071067811865475244008443621048490f;

-                internal const float Infinity = 3.402823466e+38f;

-                internal const float Epsilon = 1.192092896e-07f;

-

-                public static Vector3 MatrixToVector(Matrix m, Vector3 v)

-                {

-                        return new Vector3(

-                                Vector3.Dot(new Vector3(m.M11, m.M12, m.M13), v) + m.Translation.X,

-                                Vector3.Dot(new Vector3(m.M21, m.M22, m.M23), v) + m.Translation.Y,

-                                Vector3.Dot(new Vector3(m.M31, m.M32, m.M33), v) + m.Translation.Z

-                                );

-                }

-

-                internal static int ClosestAxis(Vector4 v)

-                {

-                        return MaxAxis(Absolute(v));

-                }

-

-                internal static Vector4 Absolute(Vector4 v)

-                {

-                        return new Vector4(Math.Abs(v.X), Math.Abs(v.Y), Math.Abs(v.Z), Math.Abs(v.W));

-                }

-

-                internal static int MaxAxis(Vector4 v)

-                {

-                        int maxIndex = -1;

-                        float maxVal = float.MinValue;

-                        if (v.X > maxVal)

-                        {

-                                maxIndex = 0;

-                                maxVal = v.X;

-                        }

-                        if (v.Y > maxVal)

-                        {

-                                maxIndex = 1;

-                                maxVal = v.Y;

-                        }

-                        if (v.Z > maxVal)

-                        {

-                                maxIndex = 2;

-                                maxVal = v.Z;

-                        }

-                        if (v.W > maxVal)

-                        {

-                                maxIndex = 3;

-                                maxVal = v.W;

-                        }

-

-                        return maxIndex;

-                }

-

-                internal static int MaxAxis(Vector3 v)

-                {

-                        return v.X < v.Y ? (v.Y < v.Z ? 2 : 1) : (v.X < v.Z ? 2 : 0);

-                }

-

-                // conservative test for overlap between two aabbs

-                internal static bool TestAabbAgainstAabb2(Vector3 aabbMinA, Vector3 aabbMaxA, Vector3 aabbMinB, Vector3 aabbMaxB)

-                {

-                        bool overlap = true;

-                        overlap = (aabbMinA.X > aabbMaxB.X || aabbMaxA.X < aabbMinB.X) ? false : overlap;

-                        overlap = (aabbMinA.Z > aabbMaxB.Z || aabbMaxA.Z < aabbMinB.Z) ? false : overlap;

-                        overlap = (aabbMinA.Y > aabbMaxB.Y || aabbMaxA.Y < aabbMinB.Y) ? false : overlap;

-                        return overlap;

-                }

-

-                internal static bool TestTriangleAgainstAabb2(Vector3[] vertices, Vector3 aabbMin, Vector3 aabbMax)

-                {

-                        Vector3 p1 = vertices[0];

-                        Vector3 p2 = vertices[1];

-                        Vector3 p3 = vertices[2];

-

-                        if (Math.Min(Math.Min(p1.X, p2.X), p3.X) > aabbMax.X) return false;

-                        if (Math.Max(Math.Max(p1.X, p2.X), p3.X) < aabbMin.X) return false;

-

-                        if (Math.Min(Math.Min(p1.Z, p2.Z), p3.Z) > aabbMax.Z) return false;

-                        if (Math.Max(Math.Max(p1.Z, p2.Z), p3.Z) < aabbMin.Z) return false;

-

-                        if (Math.Min(Math.Min(p1.Y, p2.Y), p3.Y) > aabbMax.Y) return false;

-                        if (Math.Max(Math.Max(p1.Y, p2.Y), p3.Y) < aabbMin.Y) return false;

-                        return true;

-                }

-

-                internal static void SetInterpolate3(Vector3 vA, Vector3 vB, float rt, ref Vector3 interpolated)

-                {

-                        float s = 1.0f - rt;

-                        interpolated.X = s * vA.X + rt * vB.X;

-                        interpolated.Y = s * vA.Y + rt * vB.Y;

-                        interpolated.Z = s * vA.Z + rt * vB.Z;

-                }

-

-                internal static void PlaneSpace1(Vector3 n, ref Vector3 p, ref Vector3 q)

-                {

-                        if (Math.Abs(n.Z) > Sqrt12)

-                        {

-                                // choose p in y-z plane

-                                float a = n.Y * n.Y + n.Z * n.Z;

-                                float k = 1f / (float)Math.Sqrt(a);

-                                p.X = 0;

-                                p.Y = -n.Z * k;

-                                p.Z = n.Y * k;

-                                // set q = n x p

-                                q.X = a * k;

-                                q.Y = -n.X * p.Z;

-                                q.Z = n.X * p.Y;

-                        }

-                        else

-                        {

-                                // choose p in x-y plane

-                                float a = n.X * n.X + n.Y * n.Y;

-                                float k = 1f / (float)Math.Sqrt(a);

-                                p.X = -n.Y * k;

-                                p.Y = n.X * k;

-                                p.Z = 0;

-                                // set q = n x p

-                                q.X = -n.Z * p.Y;

-                                q.Y = n.Z * p.X;

-                                q.Z = a * k;

-                        }

-                }

-

-                internal static bool RayAabb(Vector3 rayFrom,

-                                                                 Vector3 rayTo,

-                                                                 Vector3 aabbMin,

-                                                                 Vector3 aabbMax,

-                                          float param, Vector3 normal)

-                {

-                        Vector3 aabbHalfExtent = (aabbMax - aabbMin) * 0.5f;

-                        Vector3 aabbCenter = (aabbMax + aabbMin) * 0.5f;

-                        Vector3 source = rayFrom - aabbCenter;

-                        Vector3 target = rayTo - aabbCenter;

-                        int sourceOutcode = Outcode(source, aabbHalfExtent);

-                        int targetOutcode = Outcode(target, aabbHalfExtent);

-                        if ((sourceOutcode & targetOutcode) == 0x0)

-                        {

-                                float lambda_enter = 0;

-                                float lambda_exit = param;

-                                Vector3 r = target - source;

-                                float normSign = 1;

-                                Vector3 hitNormal = new Vector3();

-                                int bit = 1;

-

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

-                                {

-                                        {

-                                                if ((sourceOutcode & bit) != 0)

-                                                {

-                                                        float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X;

-                                                        if (lambda_enter <= lambda)

-                                                        {

-                                                                lambda_enter = lambda;

-                                                                hitNormal = new Vector3();

-                                                                hitNormal.X = normSign;

-                                                        }

-                                                }

-                                                else if ((targetOutcode & bit) != 0)

-                                                {

-                                                        float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X;

-                                                        SetMin(ref lambda_exit, lambda);

-                                                }

-                                                bit <<= 1;

-                                        }

-                                        {

-                                                if ((sourceOutcode & bit) != 0)

-                                                {

-                                                        float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y;

-                                                        if (lambda_enter <= lambda)

-                                                        {

-                                                                lambda_enter = lambda;

-                                                                hitNormal = new Vector3();

-                                                                hitNormal.Y = normSign;

-                                                        }

-                                                }

-                                                else if ((targetOutcode & bit) != 0)

-                                                {

-                                                        float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y;

-                                                        SetMin(ref lambda_exit, lambda);

-                                                }

-                                                bit <<= 1;

-                                        }

-                                        {

-                                                if ((sourceOutcode & bit) != 0)

-                                                {

-                                                        float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z;

-                                                        if (lambda_enter <= lambda)

-                                                        {

-                                                                lambda_enter = lambda;

-                                                                hitNormal = new Vector3();

-                                                                hitNormal.Z = normSign;

-                                                        }

-                                                }

-                                                else if ((targetOutcode & bit) != 0)

-                                                {

-                                                        float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z;

-                                                        SetMin(ref lambda_exit, lambda);

-                                                }

-                                                bit <<= 1;

-                                        }

-                                        normSign = -1;

-                                }

-                                if (lambda_enter <= lambda_exit)

-                                {

-                                        param = lambda_enter;

-                                        normal = hitNormal;

-                                        return true;

-                                }

-                        }

-                        return false;

-                }

-

-                internal static void SetMin(ref float a, float b)

-                {

-                        if (a > b)

-                                a = b;

-                }

-

-                internal static void SetMax(ref float a, float b)

-                {

-                        if (a < b)

-                                a = b;

-                }

-

-                internal static void SetMax(ref Vector3 self, Vector3 other)

-                {

-                        if (other.X > self.X)

-                                self.X = other.X;

-

-                        if (other.Y > self.Y)

-                                self.Y = other.Y;

-

-                        if (other.Z > self.Z)

-                                self.Z = other.Z;

-                }

-

-                internal static Vector3 SetMax(Vector3 self, Vector3 other)

-                {

-                        if (other.X > self.X)

-                                self.X = other.X;

-

-                        if (other.Y > self.Y)

-                                self.Y = other.Y;

-

-                        if (other.Z > self.Z)

-                                self.Z = other.Z;

-

-                        return self;

-                }

-

-                internal static void SetMin(ref Vector3 self, Vector3 other)

-                {

-                        if (other.X < self.X)

-                                self.X = other.X;

-

-                        if (other.Y < self.Y)

-                                self.Y = other.Y;

-

-                        if (other.Z < self.Z)

-                                self.Z = other.Z;

-                }

-

-                internal static Vector3 SetMin(Vector3 self, Vector3 other)

-                {

-                        if (other.X < self.X)

-                                self.X = other.X;

-

-                        if (other.Y < self.Y)

-                                self.Y = other.Y;

-

-                        if (other.Z < self.Z)

-                                self.Z = other.Z;

-

-                        return self;

-                }

-

-                internal static int Outcode(Vector3 p, Vector3 halfExtent)

-                {

-                        return (p.X < -halfExtent.X ? 0x01 : 0x0) |

-                                   (p.X > halfExtent.X ? 0x08 : 0x0) |

-                                   (p.Y < -halfExtent.Y ? 0x02 : 0x0) |

-                                   (p.Y > halfExtent.Y ? 0x10 : 0x0) |

-                                   (p.Z < -halfExtent.Z ? 0x4 : 0x0) |

-                                   (p.Z > halfExtent.Z ? 0x20 : 0x0);

-                }

-

-                internal static Matrix Absolute(Matrix m)

-                {

-                        return new Matrix(Math.Abs(m.M11), Math.Abs(m.M12), Math.Abs(m.M13), Math.Abs(m.M14),

-                                                          Math.Abs(m.M21), Math.Abs(m.M22), Math.Abs(m.M23), Math.Abs(m.M24),

-                                                          Math.Abs(m.M31), Math.Abs(m.M32), Math.Abs(m.M33), Math.Abs(m.M34),

-                                                          Math.Abs(m.M41), Math.Abs(m.M42), Math.Abs(m.M43), Math.Abs(m.M44));

-                }

-

-                internal static void SetValueByIndex(ref Vector3 v, int i, float value)

-                {

-                        if (i == 0)

-                                v.X = value;

-                        else if (i == 1)

-                                v.Y = value;

-                        else

-                                v.Z = value;

-                }

-

-                internal static float GetValueByIndex(Vector3 v, int i)

-                {

-                        if (i == 0)

-                                return v.X;

-                        else if (i == 1)

-                                return v.Y;

-                        else

-                                return v.Z;

-                }

-

-                internal static Vector3 InvXForm(Matrix m, Vector3 v)

-                {

-                        v -= m.Translation;

-                        m.Translation = new Vector3();

-                        return MathHelper.Transform(v, Matrix.Transpose(m));

-                }

-

-                internal static Matrix InverseTimes(Matrix m, Matrix t)

-                {

-                        Vector3 v = t.Translation - m.Translation;

-

-                        Matrix mat = TransposeTimes(m, t);

-                        mat.Translation = Vector3.Transform(v, m);

-                        return mat;

-                }

-

-                internal static Matrix TransposeTimes(Matrix mA, Matrix mB)

-                {

-                        return new Matrix(

-                                mA.M11 * mB.M11 + mA.M21 * mB.M21 + mA.M31 * mB.M31,

-                                mA.M11 * mB.M12 + mA.M21 * mB.M22 + mA.M31 * mB.M32,

-                                mA.M11 * mB.M13 + mA.M21 * mB.M23 + mA.M31 * mB.M33,

-                                0,

-                                mA.M12 * mB.M11 + mA.M22 * mB.M21 + mA.M32 * mB.M31,

-                                mA.M12 * mB.M12 + mA.M22 * mB.M22 + mA.M32 * mB.M32,

-                                mA.M12 * mB.M13 + mA.M22 * mB.M23 + mA.M32 * mB.M33,

-                                0,

-                                mA.M13 * mB.M11 + mA.M23 * mB.M21 + mA.M33 * mB.M31,

-                                mA.M13 * mB.M12 + mA.M23 * mB.M22 + mA.M33 * mB.M32,

-                                mA.M13 * mB.M13 + mA.M23 * mB.M23 + mA.M33 * mB.M33,

-                                0, 0, 0, 0, 1);

-                }

-

-                internal static Vector3 GetColumn(Matrix m, int column)

-                {

-                        switch (column)

-                        {

-                                case 1:

-                                        return new Vector3(m.M11, m.M21, m.M31);

-                                case 2:

-                                        return new Vector3(m.M12, m.M22, m.M32);

-                                case 3:

-                                        return new Vector3(m.M13, m.M23, m.M33);

-                                default:

-                                        throw new ArgumentOutOfRangeException("column");

-                        }

-                }

-

-                internal static Vector3 GetRow(Matrix m, int row)

-                {

-                        switch (row)

-                        {

-                                case 1:

-                                        return new Vector3(m.M11, m.M12, m.M13);

-                                case 2:

-                                        return new Vector3(m.M21, m.M22, m.M23);

-                                case 3:

-                                        return new Vector3(m.M31, m.M32, m.M33);

-                                default:

-                                        throw new ArgumentOutOfRangeException("row");

-                        }

-                }

-

-                internal static Quaternion GetRotation(Matrix m)

-                {

-                        float trace = m.M11 + m.M22 + m.M33;

-                        Quaternion q = new Quaternion();

-

-                        if (trace > 0)

-                        {

-                                float s = (float)Math.Sqrt(trace + 1.0f);

-                                q.W = s * 0.5f;

-                                s = 0.5f / s;

-

-                                q.X = (m.M32 - m.M23) * s;

-                                q.Y = (m.M13 - m.M31) * s;

-                                q.Z = (m.M21 - m.M12) * s;

-                        }

-                        else

-                        {

-                                int i = m.M11 < m.M22 ?

-                                                (m.M22 < m.M33 ? 2 : 1) :

-                                                (m.M11 < m.M33 ? 2 : 0);

-                                int j = (i + 1) % 3;

-                                int k = (i + 2) % 3;

-

-                                float s = (float)Math.Sqrt(GetElement(m, i, i) - GetElement(m, j, j) - GetElement(m, k, k) + 1.0f);

-                                SetElement(ref q, i, s * 0.5f);

-                                s = 0.5f / s;

-

-                                q.W = (GetElement(m, k, j) - GetElement(m, j, k)) * s;

-                                SetElement(ref q, j, (GetElement(m, j, i) + GetElement(m, i, j)) * s);

-                                SetElement(ref q, k, (GetElement(m, k, i) + GetElement(m, i, k)) * s);

-                        }

-                        return q;

-                }

-

-                internal static float SetElement(ref Quaternion q, int index, float value)

-                {

-                        switch (index)

-                        {

-                                case 0:

-                                        q.X = value; break;

-                                case 1:

-                                        q.Y = value; break;

-                                case 2:

-                                        q.Z = value; break;

-                                case 3:

-                                        q.W = value; break;

-                        }

-

-                        return 0;

-                }

-

-                internal static float GetElement(Quaternion q, int index)

-                {

-                        switch (index)

-                        {

-                                case 0:

-                                        return q.X;

-                                case 1:

-                                        return q.Y;

-                                case 2:

-                                        return q.Z;

-                                default:

-                                        return q.W;

-                        }

-                }

-

-                internal static float GetElement(Matrix mat, int index)

-                {

-                        int row = index % 3;

-                        int col = index / 3;

-

-                        return GetElement(mat, row, col);

-                }

-

-                internal static float GetElement(Matrix mat, int row, int col)

-                {

-                        switch (row)

-                        {

-                                case 0:

-                                        switch (col)

-                                        {

-                                                case 0:

-                                                        return mat.M11;

-                                                case 1:

-                                                        return mat.M12;

-                                                case 2:

-                                                        return mat.M13;

-                                        } break;

-                                case 1:

-                                        switch (col)

-                                        {

-                                                case 0:

-                                                        return mat.M21;

-                                                case 1:

-                                                        return mat.M22;

-                                                case 2:

-                                                        return mat.M23;

-                                        } break;

-                                case 2:

-                                        switch (col)

-                                        {

-                                                case 0:

-                                                        return mat.M31;

-                                                case 1:

-                                                        return mat.M32;

-                                                case 2:

-                                                        return mat.M33;

-                                        } break;

-                        }

-

-                        return 0;

-                }

-

-                internal static float GetElement(Vector3 v, int index)

-                {

-                        if (index == 0)

-                                return v.X;

-                        if (index == 1)

-                                return v.Y;

-                        if (index == 2)

-                                return v.Z;

-

-                        throw new ArgumentOutOfRangeException("index");

-                }

-

-                internal static void SetElement(ref Vector3 v, int index, float value)

-                {

-                        if (index == 0)

-                                v.X = value;

-                        else if (index == 1)

-                                v.Y = value;

-                        else if (index == 2)

-                                v.Z = value;

-                        else

-                                throw new ArgumentOutOfRangeException("index");

-                }

-

-                public static Matrix InvertMatrix(Matrix m)

-                {

-                        Vector3 pos = m.Translation;

-                        m.Translation = Vector3.Zero;

-                        Matrix inv = Matrix.Transpose(m);

-                        pos = Vector3.Transform(-pos, m);

-                        inv.Translation = pos;

-                        return inv;

-                }

-

-                public static Matrix GetDisplayMatrix(Matrix m)

-                {

-                        Matrix displayMatrix = m;

-                        displayMatrix.Translation = Vector3.Zero;

-                        displayMatrix = Matrix.Transpose(displayMatrix);

-                        displayMatrix.Translation = m.Translation;

-                        return displayMatrix;

-                }

-

-                internal static Vector3 Transform(Vector3 position, Matrix matrix)

-                {

-                        Vector3 vector = new Vector3();

-                        vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13)) + matrix.M41;

-                        vector.Y = (((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23)) + matrix.M42;

-                        vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33)) + matrix.M43;

-                        return vector;

-                }

-

-                internal static Vector3 TransformNormal(Vector3 position, Matrix matrix)

-                {

-                        Vector3 vector = new Vector3();

-                        vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13));

-                        vector.Y = (((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23));

-                        vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33));

-                        return vector;

-                }

-        }

-}

+/*
+  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 static class MathHelper
+        {
+                internal const float Sqrt12 = 0.7071067811865475244008443621048490f;
+                internal const float Infinity = 3.402823466e+38f;
+                internal const float Epsilon = 1.192092896e-07f;
+
+                public static Vector3 MatrixToVector(Matrix m, Vector3 v)
+                {
+                        return new Vector3(
+                                Vector3.Dot(new Vector3(m.M11, m.M12, m.M13), v) + m.Translation.X,
+                                Vector3.Dot(new Vector3(m.M21, m.M22, m.M23), v) + m.Translation.Y,
+                                Vector3.Dot(new Vector3(m.M31, m.M32, m.M33), v) + m.Translation.Z
+                                );
+                }
+
+                internal static int ClosestAxis(Vector4 v)
+                {
+                        return MaxAxis(Absolute(v));
+                }
+
+                internal static Vector4 Absolute(Vector4 v)
+                {
+                        return new Vector4(Math.Abs(v.X), Math.Abs(v.Y), Math.Abs(v.Z), Math.Abs(v.W));
+                }
+
+                internal static int MaxAxis(Vector4 v)
+                {
+                        int maxIndex = -1;
+                        float maxVal = float.MinValue;
+                        if (v.X > maxVal)
+                        {
+                                maxIndex = 0;
+                                maxVal = v.X;
+                        }
+                        if (v.Y > maxVal)
+                        {
+                                maxIndex = 1;
+                                maxVal = v.Y;
+                        }
+                        if (v.Z > maxVal)
+                        {
+                                maxIndex = 2;
+                                maxVal = v.Z;
+                        }
+                        if (v.W > maxVal)
+                        {
+                                maxIndex = 3;
+                                maxVal = v.W;
+                        }
+
+                        return maxIndex;
+                }
+
+                internal static int MaxAxis(Vector3 v)
+                {
+                        return v.X < v.Y ? (v.Y < v.Z ? 2 : 1) : (v.X < v.Z ? 2 : 0);
+                }
+
+                // conservative test for overlap between two aabbs
+                internal static bool TestAabbAgainstAabb2(Vector3 aabbMinA, Vector3 aabbMaxA, Vector3 aabbMinB, Vector3 aabbMaxB)
+                {
+                        bool overlap = true;
+                        overlap = (aabbMinA.X > aabbMaxB.X || aabbMaxA.X < aabbMinB.X) ? false : overlap;
+                        overlap = (aabbMinA.Z > aabbMaxB.Z || aabbMaxA.Z < aabbMinB.Z) ? false : overlap;
+                        overlap = (aabbMinA.Y > aabbMaxB.Y || aabbMaxA.Y < aabbMinB.Y) ? false : overlap;
+                        return overlap;
+                }
+
+                internal static bool TestTriangleAgainstAabb2(Vector3[] vertices, Vector3 aabbMin, Vector3 aabbMax)
+                {
+                        Vector3 p1 = vertices[0];
+                        Vector3 p2 = vertices[1];
+                        Vector3 p3 = vertices[2];
+
+                        if (Math.Min(Math.Min(p1.X, p2.X), p3.X) > aabbMax.X) return false;
+                        if (Math.Max(Math.Max(p1.X, p2.X), p3.X) < aabbMin.X) return false;
+
+                        if (Math.Min(Math.Min(p1.Z, p2.Z), p3.Z) > aabbMax.Z) return false;
+                        if (Math.Max(Math.Max(p1.Z, p2.Z), p3.Z) < aabbMin.Z) return false;
+
+                        if (Math.Min(Math.Min(p1.Y, p2.Y), p3.Y) > aabbMax.Y) return false;
+                        if (Math.Max(Math.Max(p1.Y, p2.Y), p3.Y) < aabbMin.Y) return false;
+                        return true;
+                }
+
+                internal static void SetInterpolate3(Vector3 vA, Vector3 vB, float rt, ref Vector3 interpolated)
+                {
+                        float s = 1.0f - rt;
+                        interpolated.X = s * vA.X + rt * vB.X;
+                        interpolated.Y = s * vA.Y + rt * vB.Y;
+                        interpolated.Z = s * vA.Z + rt * vB.Z;
+                }
+
+                internal static void PlaneSpace1(Vector3 n, ref Vector3 p, ref Vector3 q)
+                {
+                        if (Math.Abs(n.Z) > Sqrt12)
+                        {
+                                // choose p in y-z plane
+                                float a = n.Y * n.Y + n.Z * n.Z;
+                                float k = 1f / (float)Math.Sqrt(a);
+                                p.X = 0;
+                                p.Y = -n.Z * k;
+                                p.Z = n.Y * k;
+                                // set q = n x p
+                                q.X = a * k;
+                                q.Y = -n.X * p.Z;
+                                q.Z = n.X * p.Y;
+                        }
+                        else
+                        {
+                                // choose p in x-y plane
+                                float a = n.X * n.X + n.Y * n.Y;
+                                float k = 1f / (float)Math.Sqrt(a);
+                                p.X = -n.Y * k;
+                                p.Y = n.X * k;
+                                p.Z = 0;
+                                // set q = n x p
+                                q.X = -n.Z * p.Y;
+                                q.Y = n.Z * p.X;
+                                q.Z = a * k;
+                        }
+                }
+
+                internal static bool RayAabb(Vector3 rayFrom,
+                                                                 Vector3 rayTo,
+                                                                 Vector3 aabbMin,
+                                                                 Vector3 aabbMax,
+                                          float param, Vector3 normal)
+                {
+                        Vector3 aabbHalfExtent = (aabbMax - aabbMin) * 0.5f;
+                        Vector3 aabbCenter = (aabbMax + aabbMin) * 0.5f;
+                        Vector3 source = rayFrom - aabbCenter;
+                        Vector3 target = rayTo - aabbCenter;
+                        int sourceOutcode = Outcode(source, aabbHalfExtent);
+                        int targetOutcode = Outcode(target, aabbHalfExtent);
+                        if ((sourceOutcode & targetOutcode) == 0x0)
+                        {
+                                float lambda_enter = 0;
+                                float lambda_exit = param;
+                                Vector3 r = target - source;
+                                float normSign = 1;
+                                Vector3 hitNormal = new Vector3();
+                                int bit = 1;
+
+                                for (int j = 0; j < 2; j++)
+                                {
+                                        {
+                                                if ((sourceOutcode & bit) != 0)
+                                                {
+                                                        float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X;
+                                                        if (lambda_enter <= lambda)
+                                                        {
+                                                                lambda_enter = lambda;
+                                                                hitNormal = new Vector3();
+                                                                hitNormal.X = normSign;
+                                                        }
+                                                }
+                                                else if ((targetOutcode & bit) != 0)
+                                                {
+                                                        float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X;
+                                                        SetMin(ref lambda_exit, lambda);
+                                                }
+                                                bit <<= 1;
+                                        }
+                                        {
+                                                if ((sourceOutcode & bit) != 0)
+                                                {
+                                                        float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y;
+                                                        if (lambda_enter <= lambda)
+                                                        {
+                                                                lambda_enter = lambda;
+                                                                hitNormal = new Vector3();
+                                                                hitNormal.Y = normSign;
+                                                        }
+                                                }
+                                                else if ((targetOutcode & bit) != 0)
+                                                {
+                                                        float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y;
+                                                        SetMin(ref lambda_exit, lambda);
+                                                }
+                                                bit <<= 1;
+                                        }
+                                        {
+                                                if ((sourceOutcode & bit) != 0)
+                                                {
+                                                        float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z;
+                                                        if (lambda_enter <= lambda)
+                                                        {
+                                                                lambda_enter = lambda;
+                                                                hitNormal = new Vector3();
+                                                                hitNormal.Z = normSign;
+                                                        }
+                                                }
+                                                else if ((targetOutcode & bit) != 0)
+                                                {
+                                                        float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z;
+                                                        SetMin(ref lambda_exit, lambda);
+                                                }
+                                                bit <<= 1;
+                                        }
+                                        normSign = -1;
+                                }
+                                if (lambda_enter <= lambda_exit)
+                                {
+                                        param = lambda_enter;
+                                        normal = hitNormal;
+                                        return true;
+                                }
+                        }
+                        return false;
+                }
+
+                internal static void SetMin(ref float a, float b)
+                {
+                        if (a > b)
+                                a = b;
+                }
+
+                internal static void SetMax(ref float a, float b)
+                {
+                        if (a < b)
+                                a = b;
+                }
+
+                internal static void SetMax(ref Vector3 self, Vector3 other)
+                {
+                        if (other.X > self.X)
+                                self.X = other.X;
+
+                        if (other.Y > self.Y)
+                                self.Y = other.Y;
+
+                        if (other.Z > self.Z)
+                                self.Z = other.Z;
+                }
+
+                internal static Vector3 SetMax(Vector3 self, Vector3 other)
+                {
+                        if (other.X > self.X)
+                                self.X = other.X;
+
+                        if (other.Y > self.Y)
+                                self.Y = other.Y;
+
+                        if (other.Z > self.Z)
+                                self.Z = other.Z;
+
+                        return self;
+                }
+
+                internal static void SetMin(ref Vector3 self, Vector3 other)
+                {
+                        if (other.X < self.X)
+                                self.X = other.X;
+
+                        if (other.Y < self.Y)
+                                self.Y = other.Y;
+
+                        if (other.Z < self.Z)
+                                self.Z = other.Z;
+                }
+
+                internal static Vector3 SetMin(Vector3 self, Vector3 other)
+                {
+                        if (other.X < self.X)
+                                self.X = other.X;
+
+                        if (other.Y < self.Y)
+                                self.Y = other.Y;
+
+                        if (other.Z < self.Z)
+                                self.Z = other.Z;
+
+                        return self;
+                }
+
+                internal static int Outcode(Vector3 p, Vector3 halfExtent)
+                {
+                        return (p.X < -halfExtent.X ? 0x01 : 0x0) |
+                                   (p.X > halfExtent.X ? 0x08 : 0x0) |
+                                   (p.Y < -halfExtent.Y ? 0x02 : 0x0) |
+                                   (p.Y > halfExtent.Y ? 0x10 : 0x0) |
+                                   (p.Z < -halfExtent.Z ? 0x4 : 0x0) |
+                                   (p.Z > halfExtent.Z ? 0x20 : 0x0);
+                }
+
+                internal static Matrix Absolute(Matrix m)
+                {
+                        return new Matrix(Math.Abs(m.M11), Math.Abs(m.M12), Math.Abs(m.M13), Math.Abs(m.M14),
+                                                          Math.Abs(m.M21), Math.Abs(m.M22), Math.Abs(m.M23), Math.Abs(m.M24),
+                                                          Math.Abs(m.M31), Math.Abs(m.M32), Math.Abs(m.M33), Math.Abs(m.M34),
+                                                          Math.Abs(m.M41), Math.Abs(m.M42), Math.Abs(m.M43), Math.Abs(m.M44));
+                }
+
+                internal static void SetValueByIndex(ref Vector3 v, int i, float value)
+                {
+                        if (i == 0)
+                                v.X = value;
+                        else if (i == 1)
+                                v.Y = value;
+                        else
+                                v.Z = value;
+                }
+
+                internal static float GetValueByIndex(Vector3 v, int i)
+                {
+                        if (i == 0)
+                                return v.X;
+                        else if (i == 1)
+                                return v.Y;
+                        else
+                                return v.Z;
+                }
+
+                internal static Vector3 InvXForm(Matrix m, Vector3 v)
+                {
+                        v -= m.Translation;
+                        m.Translation = new Vector3();
+                        return MathHelper.Transform(v, Matrix.Transpose(m));
+                }
+
+                internal static Matrix InverseTimes(Matrix m, Matrix t)
+                {
+                        Vector3 v = t.Translation - m.Translation;
+
+                        Matrix mat = TransposeTimes(m, t);
+                        mat.Translation = Vector3.Transform(v, m);
+                        return mat;
+                }
+
+                internal static Matrix TransposeTimes(Matrix mA, Matrix mB)
+                {
+                        return new Matrix(
+                                mA.M11 * mB.M11 + mA.M21 * mB.M21 + mA.M31 * mB.M31,
+                                mA.M11 * mB.M12 + mA.M21 * mB.M22 + mA.M31 * mB.M32,
+                                mA.M11 * mB.M13 + mA.M21 * mB.M23 + mA.M31 * mB.M33,
+                                0,
+                                mA.M12 * mB.M11 + mA.M22 * mB.M21 + mA.M32 * mB.M31,
+                                mA.M12 * mB.M12 + mA.M22 * mB.M22 + mA.M32 * mB.M32,
+                                mA.M12 * mB.M13 + mA.M22 * mB.M23 + mA.M32 * mB.M33,
+                                0,
+                                mA.M13 * mB.M11 + mA.M23 * mB.M21 + mA.M33 * mB.M31,
+                                mA.M13 * mB.M12 + mA.M23 * mB.M22 + mA.M33 * mB.M32,
+                                mA.M13 * mB.M13 + mA.M23 * mB.M23 + mA.M33 * mB.M33,
+                                0, 0, 0, 0, 1);
+                }
+
+                internal static Vector3 GetColumn(Matrix m, int column)
+                {
+                        switch (column)
+                        {
+                                case 1:
+                                        return new Vector3(m.M11, m.M21, m.M31);
+                                case 2:
+                                        return new Vector3(m.M12, m.M22, m.M32);
+                                case 3:
+                                        return new Vector3(m.M13, m.M23, m.M33);
+                                default:
+                                        throw new ArgumentOutOfRangeException("column");
+                        }
+                }
+
+                internal static Vector3 GetRow(Matrix m, int row)
+                {
+                        switch (row)
+                        {
+                                case 1:
+                                        return new Vector3(m.M11, m.M12, m.M13);
+                                case 2:
+                                        return new Vector3(m.M21, m.M22, m.M23);
+                                case 3:
+                                        return new Vector3(m.M31, m.M32, m.M33);
+                                default:
+                                        throw new ArgumentOutOfRangeException("row");
+                        }
+                }
+
+                internal static Quaternion GetRotation(Matrix m)
+                {
+                        float trace = m.M11 + m.M22 + m.M33;
+                        Quaternion q = new Quaternion();
+
+                        if (trace > 0)
+                        {
+                                float s = (float)Math.Sqrt(trace + 1.0f);
+                                q.W = s * 0.5f;
+                                s = 0.5f / s;
+
+                                q.X = (m.M32 - m.M23) * s;
+                                q.Y = (m.M13 - m.M31) * s;
+                                q.Z = (m.M21 - m.M12) * s;
+                        }
+                        else
+                        {
+                                int i = m.M11 < m.M22 ?
+                                                (m.M22 < m.M33 ? 2 : 1) :
+                                                (m.M11 < m.M33 ? 2 : 0);
+                                int j = (i + 1) % 3;
+                                int k = (i + 2) % 3;
+
+                                float s = (float)Math.Sqrt(GetElement(m, i, i) - GetElement(m, j, j) - GetElement(m, k, k) + 1.0f);
+                                SetElement(ref q, i, s * 0.5f);
+                                s = 0.5f / s;
+
+                                q.W = (GetElement(m, k, j) - GetElement(m, j, k)) * s;
+                                SetElement(ref q, j, (GetElement(m, j, i) + GetElement(m, i, j)) * s);
+                                SetElement(ref q, k, (GetElement(m, k, i) + GetElement(m, i, k)) * s);
+                        }
+                        return q;
+                }
+
+                internal static float SetElement(ref Quaternion q, int index, float value)
+                {
+                        switch (index)
+                        {
+                                case 0:
+                                        q.X = value; break;
+                                case 1:
+                                        q.Y = value; break;
+                                case 2:
+                                        q.Z = value; break;
+                                case 3:
+                                        q.W = value; break;
+                        }
+
+                        return 0;
+                }
+
+                internal static float GetElement(Quaternion q, int index)
+                {
+                        switch (index)
+                        {
+                                case 0:
+                                        return q.X;
+                                case 1:
+                                        return q.Y;
+                                case 2:
+                                        return q.Z;
+                                default:
+                                        return q.W;
+                        }
+                }
+
+                internal static float GetElement(Matrix mat, int index)
+                {
+                        int row = index % 3;
+                        int col = index / 3;
+
+                        return GetElement(mat, row, col);
+                }
+
+                internal static float GetElement(Matrix mat, int row, int col)
+                {
+                        switch (row)
+                        {
+                                case 0:
+                                        switch (col)
+                                        {
+                                                case 0:
+                                                        return mat.M11;
+                                                case 1:
+                                                        return mat.M12;
+                                                case 2:
+                                                        return mat.M13;
+                                        } break;
+                                case 1:
+                                        switch (col)
+                                        {
+                                                case 0:
+                                                        return mat.M21;
+                                                case 1:
+                                                        return mat.M22;
+                                                case 2:
+                                                        return mat.M23;
+                                        } break;
+                                case 2:
+                                        switch (col)
+                                        {
+                                                case 0:
+                                                        return mat.M31;
+                                                case 1:
+                                                        return mat.M32;
+                                                case 2:
+                                                        return mat.M33;
+                                        } break;
+                        }
+
+                        return 0;
+                }
+
+                internal static float GetElement(Vector3 v, int index)
+                {
+                        if (index == 0)
+                                return v.X;
+                        if (index == 1)
+                                return v.Y;
+                        if (index == 2)
+                                return v.Z;
+
+                        throw new ArgumentOutOfRangeException("index");
+                }
+
+                internal static void SetElement(ref Vector3 v, int index, float value)
+                {
+                        if (index == 0)
+                                v.X = value;
+                        else if (index == 1)
+                                v.Y = value;
+                        else if (index == 2)
+                                v.Z = value;
+                        else
+                                throw new ArgumentOutOfRangeException("index");
+                }
+
+                public static Matrix InvertMatrix(Matrix m)
+                {
+                        Vector3 pos = m.Translation;
+                        m.Translation = Vector3.Zero;
+                        Matrix inv = Matrix.Transpose(m);
+                        pos = Vector3.Transform(-pos, m);
+                        inv.Translation = pos;
+                        return inv;
+                }
+
+                public static Matrix GetDisplayMatrix(Matrix m)
+                {
+                        Matrix displayMatrix = m;
+                        displayMatrix.Translation = Vector3.Zero;
+                        displayMatrix = Matrix.Transpose(displayMatrix);
+                        displayMatrix.Translation = m.Translation;
+                        return displayMatrix;
+                }
+
+                internal static Vector3 Transform(Vector3 position, Matrix matrix)
+                {
+                        Vector3 vector = new Vector3();
+                        vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13)) + matrix.M41;
+                        vector.Y = (((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23)) + matrix.M42;
+                        vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33)) + matrix.M43;
+                        return vector;
+                }
+
+                internal static Vector3 TransformNormal(Vector3 position, Matrix matrix)
+                {
+                        Vector3 vector = new Vector3();
+                        vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13));
+                        vector.Y = (((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23));
+                        vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33));
+                        return vector;
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs
index 04e42d9..93a99f2 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs
@@ -1,116 +1,116 @@
-/*

-  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

-{

-        internal static class MatrixOperations

-        {

-                public static void SetRotation(ref Matrix m, Quaternion q)

-                {

-                        float d = q.LengthSquared();

-                        BulletDebug.Assert(d != 0);

-                        float s = 2f / d;

-                        float xs = q.X * s, ys = q.Y * s, zs = q.Z * s;

-                        float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs;

-                        float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs;

-                        float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs;

-                        m = new Matrix(1 - (yy + zz), xy - wz, xz + wy, 0,

-                                                        xy + wz, 1 - (xx + zz), yz - wx, 0,

-                                                        xz - wy, yz + wx, 1 - (xx + yy), 0,

-                                                        m.M41, m.M42, m.M43, 1);

-                }

-

-                public static Quaternion GetRotation(Matrix m)

-                {

-                        Quaternion q = new Quaternion();

-

-                        float trace = m.M11 + m.M22 + m.M33;

-

-                        if (trace > 0)

-                        {

-                                float s = (float)Math.Sqrt(trace + 1);

-                                q.W = s * 0.5f;

-                                s = 0.5f / s;

-

-                                q.X = (m.M32 - m.M23) * s;

-                                q.Y = (m.M13 - m.M31) * s;

-                                q.Z = (m.M21 - m.M12) * s;

-                        }

-                        else

-                        {

-                                int i = m.M11 < m.M22 ?

-                                        (m.M22 < m.M33 ? 2 : 1) :

-                                        (m.M11 < m.M33 ? 2 : 0);

-                                int j = (i + 1) % 3;

-                                int k = (i + 2) % 3;

-

-                                float s = (float)Math.Sqrt(MathHelper.GetElement(m, i, i) - MathHelper.GetElement(m, j, j) - MathHelper.GetElement(m, k, k) + 1);

-                                MathHelper.SetElement(ref q, i, s * 0.5f);

-                                s = 0.5f / s;

-

-                                q.W = (MathHelper.GetElement(m, k, j) - MathHelper.GetElement(m, j, k)) * s;

-                                MathHelper.SetElement(ref q, j, (MathHelper.GetElement(m, j, i) + MathHelper.GetElement(m, i, j)) * s);

-                                MathHelper.SetElement(ref q, k, (MathHelper.GetElement(m, k, i) + MathHelper.GetElement(m, i, k)) * s);

-                        }

-

-                        return q;

-                }

-

-                public static Matrix Scaled(Matrix m, Vector3 v)

-                {

-                        return new Matrix(  m.M11 * v.X, m.M12 * v.Y, m.M13 * v.Z, 0,

-                                                                m.M21 * v.X, m.M22 * v.Y, m.M23 * v.Z, 0,

-                                                                m.M31 * v.X, m.M32 * v.Y, m.M33 * v.Z, 0,

-                                                                0, 0, 0, 1);

-                }

-                

-                public static Matrix Multiply(Matrix a, Matrix b)

-                {

-                        /*return btMatrix3x3(

-                        m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]),

-                        m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]),

-                        m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2]));*/

-                        return new Matrix(

-                                Dot(b, 0, MathHelper.GetRow(a, 1)), Dot(b, 1, MathHelper.GetRow(a, 1)), Dot(b, 2, MathHelper.GetRow(a, 1)), 0,

-                                Dot(b, 0, MathHelper.GetRow(a, 2)), Dot(b, 1, MathHelper.GetRow(a, 2)), Dot(b, 2, MathHelper.GetRow(a, 2)), 0,

-                                Dot(b, 0, MathHelper.GetRow(a, 3)), Dot(b, 1, MathHelper.GetRow(a, 3)), Dot(b, 2, MathHelper.GetRow(a, 3)), 0,

-                                0, 0, 0, 1);

-                }

-

-                public static float Dot(Matrix m, int c, Vector3 v) 

-                {

-                        return MathHelper.GetElement(m, 0, c) * v.X + MathHelper.GetElement(m, 1, c) * v.Y + MathHelper.GetElement(m, 2, c) * v.Z;

-                }

-

-                public static Matrix Transpose(Matrix m)

-                {

-                        return new Matrix(      m.M11, m.M21, m.M31, 0,

-                                                                m.M12, m.M22, m.M32, 0,

-                                                                m.M13, m.M23, m.M33, 0,

-                                                                0, 0, 0, 1);

-                }

-        }

-}

+/*
+  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
+{
+        internal static class MatrixOperations
+        {
+                public static void SetRotation(ref Matrix m, Quaternion q)
+                {
+                        float d = q.LengthSquared();
+                        BulletDebug.Assert(d != 0);
+                        float s = 2f / d;
+                        float xs = q.X * s, ys = q.Y * s, zs = q.Z * s;
+                        float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs;
+                        float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs;
+                        float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs;
+                        m = new Matrix(1 - (yy + zz), xy - wz, xz + wy, 0,
+                                                        xy + wz, 1 - (xx + zz), yz - wx, 0,
+                                                        xz - wy, yz + wx, 1 - (xx + yy), 0,
+                                                        m.M41, m.M42, m.M43, 1);
+                }
+
+                public static Quaternion GetRotation(Matrix m)
+                {
+                        Quaternion q = new Quaternion();
+
+                        float trace = m.M11 + m.M22 + m.M33;
+
+                        if (trace > 0)
+                        {
+                                float s = (float)Math.Sqrt(trace + 1);
+                                q.W = s * 0.5f;
+                                s = 0.5f / s;
+
+                                q.X = (m.M32 - m.M23) * s;
+                                q.Y = (m.M13 - m.M31) * s;
+                                q.Z = (m.M21 - m.M12) * s;
+                        }
+                        else
+                        {
+                                int i = m.M11 < m.M22 ?
+                                        (m.M22 < m.M33 ? 2 : 1) :
+                                        (m.M11 < m.M33 ? 2 : 0);
+                                int j = (i + 1) % 3;
+                                int k = (i + 2) % 3;
+
+                                float s = (float)Math.Sqrt(MathHelper.GetElement(m, i, i) - MathHelper.GetElement(m, j, j) - MathHelper.GetElement(m, k, k) + 1);
+                                MathHelper.SetElement(ref q, i, s * 0.5f);
+                                s = 0.5f / s;
+
+                                q.W = (MathHelper.GetElement(m, k, j) - MathHelper.GetElement(m, j, k)) * s;
+                                MathHelper.SetElement(ref q, j, (MathHelper.GetElement(m, j, i) + MathHelper.GetElement(m, i, j)) * s);
+                                MathHelper.SetElement(ref q, k, (MathHelper.GetElement(m, k, i) + MathHelper.GetElement(m, i, k)) * s);
+                        }
+
+                        return q;
+                }
+
+                public static Matrix Scaled(Matrix m, Vector3 v)
+                {
+                        return new Matrix(  m.M11 * v.X, m.M12 * v.Y, m.M13 * v.Z, 0,
+                                                                m.M21 * v.X, m.M22 * v.Y, m.M23 * v.Z, 0,
+                                                                m.M31 * v.X, m.M32 * v.Y, m.M33 * v.Z, 0,
+                                                                0, 0, 0, 1);
+                }
+                
+                public static Matrix Multiply(Matrix a, Matrix b)
+                {
+                        /*return btMatrix3x3(
+                        m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]),
+                        m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]),
+                        m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2]));*/
+                        return new Matrix(
+                                Dot(b, 0, MathHelper.GetRow(a, 1)), Dot(b, 1, MathHelper.GetRow(a, 1)), Dot(b, 2, MathHelper.GetRow(a, 1)), 0,
+                                Dot(b, 0, MathHelper.GetRow(a, 2)), Dot(b, 1, MathHelper.GetRow(a, 2)), Dot(b, 2, MathHelper.GetRow(a, 2)), 0,
+                                Dot(b, 0, MathHelper.GetRow(a, 3)), Dot(b, 1, MathHelper.GetRow(a, 3)), Dot(b, 2, MathHelper.GetRow(a, 3)), 0,
+                                0, 0, 0, 1);
+                }
+
+                public static float Dot(Matrix m, int c, Vector3 v) 
+                {
+                        return MathHelper.GetElement(m, 0, c) * v.X + MathHelper.GetElement(m, 1, c) * v.Y + MathHelper.GetElement(m, 2, c) * v.Z;
+                }
+
+                public static Matrix Transpose(Matrix m)
+                {
+                        return new Matrix(      m.M11, m.M21, m.M31, 0,
+                                                                m.M12, m.M22, m.M32, 0,
+                                                                m.M13, m.M23, m.M33, 0,
+                                                                0, 0, 0, 1);
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs
index 4039177..407a285 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs
@@ -1,44 +1,44 @@
-/*

-  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 MotionState

-        {

-                /*public abstract Vector3 getWorldPosition();

-                public abstract Vector3 getWorldScaling();

-                public abstract Quaternion getWorldOrientation();

-

-                public abstract void setWorldPosition(Vector3 worldPos);

-                public abstract void setWorldOrientation(Quaternion orn);

-

-                public abstract void calculateWorldTransformations();*/

-

-                public abstract void GetWorldTransform(out Matrix worldTransform);

-                //Bullet only calls the update of worldtransform for active objects

-                public abstract void SetWorldTransform(Matrix worldTransform);  

-        }

-}

+/*
+  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 MotionState
+        {
+                /*public abstract Vector3 getWorldPosition();
+                public abstract Vector3 getWorldScaling();
+                public abstract Quaternion getWorldOrientation();
+
+                public abstract void setWorldPosition(Vector3 worldPos);
+                public abstract void setWorldOrientation(Quaternion orn);
+
+                public abstract void calculateWorldTransformations();*/
+
+                public abstract void GetWorldTransform(out Matrix worldTransform);
+                //Bullet only calls the update of worldtransform for active objects
+                public abstract void SetWorldTransform(Matrix worldTransform);  
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs
index 35c9fed..0f59e87 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs
@@ -1,88 +1,88 @@
-/*

-  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;

-

-namespace XnaDevRu.BulletX.LinearMath

-{

-    internal abstract class QuadWord

-    {

-        private float x;

-        private float y;

-        private float z;

-        private float w;

-

-        public float X { get { return x; } set { x = value; } }

-        public float Y { get { return y; } set { y = value; } }

-        public float Z { get { return z; } set { z = value; } }

-        public float W { get { return w; } set { w = value; } }

-

-        public QuadWord() { }

-

-        public QuadWord(float x, float y, float z, float w)

-        {

-            X = x;

-            Y = y;

-            Z = z;

-            W = w;

-        }

-

-        public QuadWord(float x, float y, float z)

-        {

-            X = x;

-            Y = y;

-            Z = z;

-            W = 0;

-        }

-

-        public void SetMax(QuadWord other)

-        {

-            if (other.X > X)

-                X = other.X;

-

-            if (other.Y > Y)

-                Y = other.Y;

-

-            if (other.Z > Z)

-                Z = other.Z;

-

-            if (other.W > W)

-                W = other.W;

-        }

-

-        public void SetMin(QuadWord other)

-        {

-            if (other.X < X)

-                X = other.X;

-

-            if (other.Y < Y)

-                Y = other.Y;

-

-            if (other.Z < Z)

-                Z = other.Z;

-

-            if (other.W < W)

-                W = other.W;

-        }

-    }

-}

+/*
+  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;
+
+namespace XnaDevRu.BulletX.LinearMath
+{
+    internal abstract class QuadWord
+    {
+        private float x;
+        private float y;
+        private float z;
+        private float w;
+
+        public float X { get { return x; } set { x = value; } }
+        public float Y { get { return y; } set { y = value; } }
+        public float Z { get { return z; } set { z = value; } }
+        public float W { get { return w; } set { w = value; } }
+
+        public QuadWord() { }
+
+        public QuadWord(float x, float y, float z, float w)
+        {
+            X = x;
+            Y = y;
+            Z = z;
+            W = w;
+        }
+
+        public QuadWord(float x, float y, float z)
+        {
+            X = x;
+            Y = y;
+            Z = z;
+            W = 0;
+        }
+
+        public void SetMax(QuadWord other)
+        {
+            if (other.X > X)
+                X = other.X;
+
+            if (other.Y > Y)
+                Y = other.Y;
+
+            if (other.Z > Z)
+                Z = other.Z;
+
+            if (other.W > W)
+                W = other.W;
+        }
+
+        public void SetMin(QuadWord other)
+        {
+            if (other.X < X)
+                X = other.X;
+
+            if (other.Y < Y)
+                Y = other.Y;
+
+            if (other.Z < Z)
+                Z = other.Z;
+
+            if (other.W < W)
+                W = other.W;
+        }
+    }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs
index 5425a05..0685d4a 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs
@@ -1,198 +1,198 @@
-/*

-  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;

-

-namespace XnaDevRu.BulletX.LinearMath

-{

-        internal class Quaternion : QuadWord

-        {

-                public Quaternion() { }

-

-                public Quaternion(float x, float y, float z, float w)

-                        : base(x, y, z, w) { }

-

-                public Quaternion(Vector3 axis, float angle)

-                {

-                        SetRotation(axis, angle);

-                }

-

-                public Quaternion(float yaw, float pitch, float roll)

-                {

-                        SetEuler(yaw, pitch, roll);

-                }

-

-                public void SetRotation(Vector3 axis, float angle)

-                {

-                        float d = axis.Length();

-                        if (d == 0) throw new DivideByZeroException();

-                        float s = (float)Math.Sin(angle * 0.5f) / d;

-                        X = axis.X * s;

-                        Y = axis.Y * s;

-                        Z = axis.Z * s;

-                        W = (float)Math.Cos(angle * 0.5f);

-                }

-

-                public void SetEuler(float yaw, float pitch, float roll)

-                {

-                        float halfYaw = yaw * 0.5f;

-                        float halfPitch = pitch * 0.5f;

-                        float halfRoll = roll * 0.5f;

-                        float cosYaw = (float)Math.Cos(halfYaw);

-                        float sinYaw = (float)Math.Sin(halfYaw);

-                        float cosPitch = (float)Math.Cos(halfPitch);

-                        float sinPitch = (float)Math.Sin(halfPitch);

-                        float cosRoll = (float)Math.Cos(halfRoll);

-                        float sinRoll = (float)Math.Sin(halfRoll);

-                        X = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;

-                        Y = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;

-                        Z = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;

-                        W = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;

-                }

-

-                public float LengthSquared()

-                {

-                        return Dot(this, this);

-                }

-

-                public float Length()

-                {

-                        return (float)Math.Sqrt(LengthSquared());

-                }

-

-                public float Angle()

-                {

-                        return 2f * (float)Math.Acos(W);

-                }

-

-                public static float Angle(Quaternion a, Quaternion b)

-                {

-                        float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared());

-                        if (s == 0) throw new DivideByZeroException();

-                        return (float)Math.Acos(Dot(a, b) / s);

-                }

-

-                public static Quaternion Farthest(Quaternion a, Quaternion b)

-                {

-                        Quaternion diff, sum;

-                        diff = a - b;

-                        sum = a + b;

-                        if (Dot(diff, diff) > Dot(sum, sum))

-                                return b;

-                        return -b;

-                }

-

-                public static Quaternion Slerp(Quaternion a, Quaternion b, float c)

-                {

-                        float theta = Angle(a, b);

-                        if (theta != 0)

-                        {

-                                float d = 1f / (float)Math.Sin(theta);

-                                float s0 = (float)Math.Sin((1f - c) * theta);

-                                float s1 = (float)Math.Sin(c * theta);

-                                return new Quaternion(

-                                        (a.X * s0 + b.X * s1) * d,

-                                        (a.Y * s0 + b.Y * s1) * d,

-                                        (a.Z * s0 + b.Z * s1) * d,

-                                        (a.W * s0 + b.W * s1) * d);

-                        }

-                        else

-                        {

-                                return a;

-                        }

-                }

-

-                public static Quaternion Inverse(Quaternion a)

-                {

-                        return new Quaternion(a.X, a.Y, a.Z, -a.W);

-                }

-

-                public static Quaternion Normalize(Quaternion a)

-                {

-                        return a / a.Length();

-                }

-

-                public static float Dot(Quaternion a, Quaternion b)

-                {

-                        return a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W;

-                }

-

-                public static Quaternion operator +(Quaternion a, Quaternion b)

-                {

-                        return new Quaternion(a.X + b.X, a.Y + b.Y, a.Z + b.Z, a.W + b.W);

-                }

-

-                public static Quaternion operator -(Quaternion a, Quaternion b)

-                {

-                        return new Quaternion(a.X - b.X, a.Y - b.Y, a.Z - b.Z, a.W - b.W);

-                }

-

-                public static Quaternion operator -(Quaternion a)

-                {

-                        return new Quaternion(-a.X, -a.Y, -a.Z, -a.W);

-                }

-

-                public static Quaternion operator *(Quaternion a, float b)

-                {

-                        return new Quaternion(a.X * b, a.Y * b, a.Z * b, a.W * b);

-                }

-

-                public static Quaternion operator *(Quaternion a, Quaternion b)

-                {

-                        return new Quaternion(

-                                a.W * b.X + a.X * b.W + a.Y * b.Z - a.Z * b.Y,

-                                a.W * b.Y + a.Y * b.W + a.Z * b.X - a.X * b.Z,

-                                a.W * b.Z + a.Z * b.W + a.X * b.Y - a.Y * b.X,

-                                a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z);

-                }

-

-                public static Quaternion operator *(Quaternion a, Vector3 b)

-                {

-                        return new Quaternion(

-                                a.W * b.X + a.Y * b.Z - a.Z * b.Y,

-                                a.W * b.Y + a.Z * b.X - a.X * b.Z,

-                                a.W * b.Z + a.X * b.Y - a.Y * b.X,

-                                -a.X * b.X - a.Y * b.Y - a.Z * b.Z);

-                }

-

-                public static Quaternion operator *(Vector3 w, Quaternion q)

-                {

-                        return new Quaternion(

-                                w.X * q.W + w.Y * q.Z - w.Z * q.Y,

-                                w.Y * q.W + w.Z * q.X - w.X * q.Z,

-                                w.Z * q.W + w.X * q.Y - w.Y * q.X,

-                                -w.X * q.X - w.Y * q.Y - w.Z * q.Z);

-                }

-

-                public static Quaternion operator /(Quaternion a, float b)

-                {

-                        if (b == 0) throw new DivideByZeroException();

-                        return new Quaternion(a.X / b, a.Y / b, a.Z / b, a.W / b);

-                }

-

-                public static explicit operator MonoXnaCompactMaths.Quaternion(Quaternion a)

-                {

-                        return new MonoXnaCompactMaths.Quaternion(a.X, a.Y, a.Z, a.W);

-                }

-        }

-}

+/*
+  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;
+
+namespace XnaDevRu.BulletX.LinearMath
+{
+        internal class Quaternion : QuadWord
+        {
+                public Quaternion() { }
+
+                public Quaternion(float x, float y, float z, float w)
+                        : base(x, y, z, w) { }
+
+                public Quaternion(Vector3 axis, float angle)
+                {
+                        SetRotation(axis, angle);
+                }
+
+                public Quaternion(float yaw, float pitch, float roll)
+                {
+                        SetEuler(yaw, pitch, roll);
+                }
+
+                public void SetRotation(Vector3 axis, float angle)
+                {
+                        float d = axis.Length();
+                        if (d == 0) throw new DivideByZeroException();
+                        float s = (float)Math.Sin(angle * 0.5f) / d;
+                        X = axis.X * s;
+                        Y = axis.Y * s;
+                        Z = axis.Z * s;
+                        W = (float)Math.Cos(angle * 0.5f);
+                }
+
+                public void SetEuler(float yaw, float pitch, float roll)
+                {
+                        float halfYaw = yaw * 0.5f;
+                        float halfPitch = pitch * 0.5f;
+                        float halfRoll = roll * 0.5f;
+                        float cosYaw = (float)Math.Cos(halfYaw);
+                        float sinYaw = (float)Math.Sin(halfYaw);
+                        float cosPitch = (float)Math.Cos(halfPitch);
+                        float sinPitch = (float)Math.Sin(halfPitch);
+                        float cosRoll = (float)Math.Cos(halfRoll);
+                        float sinRoll = (float)Math.Sin(halfRoll);
+                        X = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
+                        Y = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
+                        Z = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
+                        W = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
+                }
+
+                public float LengthSquared()
+                {
+                        return Dot(this, this);
+                }
+
+                public float Length()
+                {
+                        return (float)Math.Sqrt(LengthSquared());
+                }
+
+                public float Angle()
+                {
+                        return 2f * (float)Math.Acos(W);
+                }
+
+                public static float Angle(Quaternion a, Quaternion b)
+                {
+                        float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared());
+                        if (s == 0) throw new DivideByZeroException();
+                        return (float)Math.Acos(Dot(a, b) / s);
+                }
+
+                public static Quaternion Farthest(Quaternion a, Quaternion b)
+                {
+                        Quaternion diff, sum;
+                        diff = a - b;
+                        sum = a + b;
+                        if (Dot(diff, diff) > Dot(sum, sum))
+                                return b;
+                        return -b;
+                }
+
+                public static Quaternion Slerp(Quaternion a, Quaternion b, float c)
+                {
+                        float theta = Angle(a, b);
+                        if (theta != 0)
+                        {
+                                float d = 1f / (float)Math.Sin(theta);
+                                float s0 = (float)Math.Sin((1f - c) * theta);
+                                float s1 = (float)Math.Sin(c * theta);
+                                return new Quaternion(
+                                        (a.X * s0 + b.X * s1) * d,
+                                        (a.Y * s0 + b.Y * s1) * d,
+                                        (a.Z * s0 + b.Z * s1) * d,
+                                        (a.W * s0 + b.W * s1) * d);
+                        }
+                        else
+                        {
+                                return a;
+                        }
+                }
+
+                public static Quaternion Inverse(Quaternion a)
+                {
+                        return new Quaternion(a.X, a.Y, a.Z, -a.W);
+                }
+
+                public static Quaternion Normalize(Quaternion a)
+                {
+                        return a / a.Length();
+                }
+
+                public static float Dot(Quaternion a, Quaternion b)
+                {
+                        return a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W;
+                }
+
+                public static Quaternion operator +(Quaternion a, Quaternion b)
+                {
+                        return new Quaternion(a.X + b.X, a.Y + b.Y, a.Z + b.Z, a.W + b.W);
+                }
+
+                public static Quaternion operator -(Quaternion a, Quaternion b)
+                {
+                        return new Quaternion(a.X - b.X, a.Y - b.Y, a.Z - b.Z, a.W - b.W);
+                }
+
+                public static Quaternion operator -(Quaternion a)
+                {
+                        return new Quaternion(-a.X, -a.Y, -a.Z, -a.W);
+                }
+
+                public static Quaternion operator *(Quaternion a, float b)
+                {
+                        return new Quaternion(a.X * b, a.Y * b, a.Z * b, a.W * b);
+                }
+
+                public static Quaternion operator *(Quaternion a, Quaternion b)
+                {
+                        return new Quaternion(
+                                a.W * b.X + a.X * b.W + a.Y * b.Z - a.Z * b.Y,
+                                a.W * b.Y + a.Y * b.W + a.Z * b.X - a.X * b.Z,
+                                a.W * b.Z + a.Z * b.W + a.X * b.Y - a.Y * b.X,
+                                a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z);
+                }
+
+                public static Quaternion operator *(Quaternion a, Vector3 b)
+                {
+                        return new Quaternion(
+                                a.W * b.X + a.Y * b.Z - a.Z * b.Y,
+                                a.W * b.Y + a.Z * b.X - a.X * b.Z,
+                                a.W * b.Z + a.X * b.Y - a.Y * b.X,
+                                -a.X * b.X - a.Y * b.Y - a.Z * b.Z);
+                }
+
+                public static Quaternion operator *(Vector3 w, Quaternion q)
+                {
+                        return new Quaternion(
+                                w.X * q.W + w.Y * q.Z - w.Z * q.Y,
+                                w.Y * q.W + w.Z * q.X - w.X * q.Z,
+                                w.Z * q.W + w.X * q.Y - w.Y * q.X,
+                                -w.X * q.X - w.Y * q.Y - w.Z * q.Z);
+                }
+
+                public static Quaternion operator /(Quaternion a, float b)
+                {
+                        if (b == 0) throw new DivideByZeroException();
+                        return new Quaternion(a.X / b, a.Y / b, a.Z / b, a.W / b);
+                }
+
+                public static explicit operator MonoXnaCompactMaths.Quaternion(Quaternion a)
+                {
+                        return new MonoXnaCompactMaths.Quaternion(a.X, a.Y, a.Z, a.W);
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs
index 031faab..ccf92a0 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs
@@ -1,102 +1,102 @@
-/*

-  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 static class TransformUtil

-        {

-                //const float AngularMotionTreshold = 0.5f * MonoXnaCompactMaths.MathHelper.PiOver2;

-        const float AngularMotionTreshold = 0.5f * (float)Math.PI / 2.0f;

-

-                public static void IntegrateTransform(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, ref Matrix predictedTransform)

-                {

-                        predictedTransform.Translation = currentTransform.Translation + linearVelocity * timeStep;

-                        //exponential map

-                        Vector3 axis;

-                        float angle = angularVelocity.Length();

-                        //limit the angular motion

-                        if (angle * timeStep > AngularMotionTreshold)

-                        {

-                                angle = AngularMotionTreshold / timeStep;

-                        }

-

-                        if (angle < 0.001f)

-                        {

-                                // use Taylor's expansions of sync function

-                                axis = angularVelocity * (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * angle * angle);

-                        }

-                        else

-                        {

-                                // sync(fAngle) = sin(c*fAngle)/t

-                                axis = angularVelocity * ((float)Math.Sin(0.5f * angle * timeStep) / angle);

-                        }

-                        Quaternion dorn = new Quaternion(axis.X, axis.Y, axis.Z, (float)Math.Cos(angle * timeStep * 0.5f));

-                        Quaternion ornA = MatrixOperations.GetRotation(currentTransform);

-

-                        Quaternion predictedOrn = dorn * ornA;

-                        predictedOrn.Normalize();

-

-                        MatrixOperations.SetRotation(ref predictedTransform, predictedOrn);

-

-                        Matrix test = Matrix.CreateFromQuaternion(predictedOrn);

-                }

-

-                public static void CalculateVelocity(Matrix transformA, Matrix transformB, float timeStep, ref Vector3 linearVelocity, ref Vector3 angularVelocity)

-                {

-                        linearVelocity = (transformB.Translation - transformA.Translation) / timeStep;

-            Matrix dmat = transformB * MathHelper.InvertMatrix(transformA);

-                        Quaternion dorn = Quaternion.CreateFromRotationMatrix(dmat);

-

-                        Vector3 axis;

-                        float angle = 2 * (float)Math.Acos(dorn.W);

-                        axis = new Vector3(dorn.X, dorn.Y, dorn.Z);

-                        //axis[3] = 0.f;

-                        //check for axis length

-                        float len = axis.LengthSquared();

-                        if (len < MathHelper.Epsilon * MathHelper.Epsilon)

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

-                        else

-                                axis /= (float)Math.Sqrt(len);

-

-                        angularVelocity = axis * angle / timeStep;

-                }

-

-                public static void CalculateDiffAxisAngle(Matrix transformA, Matrix transformB, out Vector3 axis, out float angle)

-                {

-                        Matrix dmat = transformB * MathHelper.InvertMatrix(transformA);

-                        Quaternion dorn = MathHelper.GetRotation(dmat);

-

-                        angle = 2f * (float)Math.Acos(dorn.W);

-                        axis = new Vector3(dorn.X, dorn.Y, dorn.Z);

-                        //check for axis length

-                        float len = axis.LengthSquared();

-                        if (len < MathHelper.Epsilon * MathHelper.Epsilon)

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

-                        else

-                                axis /= (float)Math.Sqrt(len);

-                }

-        }

-}

+/*
+  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 static class TransformUtil
+        {
+                //const float AngularMotionTreshold = 0.5f * MonoXnaCompactMaths.MathHelper.PiOver2;
+        const float AngularMotionTreshold = 0.5f * (float)Math.PI / 2.0f;
+
+                public static void IntegrateTransform(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, ref Matrix predictedTransform)
+                {
+                        predictedTransform.Translation = currentTransform.Translation + linearVelocity * timeStep;
+                        //exponential map
+                        Vector3 axis;
+                        float angle = angularVelocity.Length();
+                        //limit the angular motion
+                        if (angle * timeStep > AngularMotionTreshold)
+                        {
+                                angle = AngularMotionTreshold / timeStep;
+                        }
+
+                        if (angle < 0.001f)
+                        {
+                                // use Taylor's expansions of sync function
+                                axis = angularVelocity * (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * angle * angle);
+                        }
+                        else
+                        {
+                                // sync(fAngle) = sin(c*fAngle)/t
+                                axis = angularVelocity * ((float)Math.Sin(0.5f * angle * timeStep) / angle);
+                        }
+                        Quaternion dorn = new Quaternion(axis.X, axis.Y, axis.Z, (float)Math.Cos(angle * timeStep * 0.5f));
+                        Quaternion ornA = MatrixOperations.GetRotation(currentTransform);
+
+                        Quaternion predictedOrn = dorn * ornA;
+                        predictedOrn.Normalize();
+
+                        MatrixOperations.SetRotation(ref predictedTransform, predictedOrn);
+
+                        Matrix test = Matrix.CreateFromQuaternion(predictedOrn);
+                }
+
+                public static void CalculateVelocity(Matrix transformA, Matrix transformB, float timeStep, ref Vector3 linearVelocity, ref Vector3 angularVelocity)
+                {
+                        linearVelocity = (transformB.Translation - transformA.Translation) / timeStep;
+            Matrix dmat = transformB * MathHelper.InvertMatrix(transformA);
+                        Quaternion dorn = Quaternion.CreateFromRotationMatrix(dmat);
+
+                        Vector3 axis;
+                        float angle = 2 * (float)Math.Acos(dorn.W);
+                        axis = new Vector3(dorn.X, dorn.Y, dorn.Z);
+                        //axis[3] = 0.f;
+                        //check for axis length
+                        float len = axis.LengthSquared();
+                        if (len < MathHelper.Epsilon * MathHelper.Epsilon)
+                                axis = new Vector3(1f, 0f, 0f);
+                        else
+                                axis /= (float)Math.Sqrt(len);
+
+                        angularVelocity = axis * angle / timeStep;
+                }
+
+                public static void CalculateDiffAxisAngle(Matrix transformA, Matrix transformB, out Vector3 axis, out float angle)
+                {
+                        Matrix dmat = transformB * MathHelper.InvertMatrix(transformA);
+                        Quaternion dorn = MathHelper.GetRotation(dmat);
+
+                        angle = 2f * (float)Math.Acos(dorn.W);
+                        axis = new Vector3(dorn.X, dorn.Y, dorn.Z);
+                        //check for axis length
+                        float len = axis.LengthSquared();
+                        if (len < MathHelper.Epsilon * MathHelper.Epsilon)
+                                axis = new Vector3(1f, 0f, 0f);
+                        else
+                                axis /= (float)Math.Sqrt(len);
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs
index 79b1262..adeec25 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs
@@ -1,221 +1,221 @@
-/*

-  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;

-

-namespace XnaDevRu.BulletX.LinearMath

-{

-        internal class Vector3 : QuadWord

-        {

-                public Vector3() { }

-

-                public Vector3(float x, float y, float z)

-                        : base(x, y, z) { }

-

-                public void SetInterpolate3(Vector3 a, Vector3 b, float c)

-                {

-                        float s = 1.0f - c;

-                        X = s * a.X + c * b.X;

-                        Y = s * a.Y + c * b.Y;

-                        Z = s * a.Z + c * b.Z;

-                }

-

-                public float LengthSquared()

-                {

-                        return Dot(this, this);

-                }

-

-                public float Length()

-                {

-                        return (float)Math.Sqrt(LengthSquared());

-                }

-

-                public int MinAxis()

-                {

-                        return X < Y ? (X < Z ? 0 : 2) : (Y < Z ? 1 : 2);

-                }

-

-                public int MaxAxis()

-                {

-                        return X < Y ? (Y < Z ? 2 : 1) : (X < Z ? 2 : 0);

-                }

-

-                public int FurthestAxis()

-                {

-                        return Absolute(this).MinAxis();

-                }

-

-                public int ClosestAxis()

-                {

-                        return Absolute(this).MaxAxis();

-                }

-

-                public Vector3 Rotate(Vector3 axis, float angle)

-                {

-                        Vector3 o = axis * Dot(axis, this);

-                        Vector3 x = this - o;

-                        Vector3 y = Cross(axis, this);

-

-                        return (o + x * (float)Math.Cos(angle) + y * (float)Math.Sin(angle));

-                }

-

-                public static Vector3 Lerp(Vector3 a, Vector3 b, float c)

-                {

-                        return new Vector3(

-                                a.X + (b.X - a.X) * c,

-                                a.Y + (b.Y - a.Y) * c,

-                                a.Z + (b.Z - a.Z) * c);

-                }

-

-                public static float Angle(Vector3 a, Vector3 b)

-                {

-                        float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared());

-                        if (s == 0) throw new DivideByZeroException();

-                        return (float)Math.Acos(Dot(a, b) / s);

-                }

-

-                public static Vector3 Absolute(Vector3 a)

-                {

-                        return new Vector3(

-                                Math.Abs(a.X),

-                                Math.Abs(a.Y),

-                                Math.Abs(a.Z));

-                }

-

-                public static Vector3 Normalize(Vector3 a)

-                {

-                        return a / a.Length();

-                }

-

-                public static Vector3 Cross(Vector3 a, Vector3 b)

-                {

-                        return new Vector3(

-                                a.Y * b.Z - a.Z * b.Y,

-                                a.Z * b.X - a.X * b.Z,

-                                a.X * b.Y - a.Y * b.X);

-                }

-

-                public static float Dot(Vector3 a, Vector3 b)

-                {

-                        return a.X * b.X + a.Y * b.Y + a.Z * b.Z;

-                }

-

-                public static float Triple(Vector3 a, Vector3 b, Vector3 c)

-                {

-                        return a.X * (b.Y * c.Z - b.Z * c.Y) +

-                                a.Y * (b.Z * c.X - b.X * c.Z) +

-                                a.Z * (b.X * c.Y - b.Y * c.X);

-                }

-

-                public static float Distance(Vector3 a, Vector3 b)

-                {

-                        return (b - a).Length();

-                }

-

-                public static float DistanceSquared(Vector3 a, Vector3 b)

-                {

-                        return (b - a).LengthSquared();

-                }

-

-                public static Vector3 Rotate(Vector3 a, Vector3 axis, float angle)

-                {

-                        Vector3 o = axis * Dot(axis, a);

-                        Vector3 x = a - o;

-                        Vector3 y = Cross(axis, a);

-

-                        return (o + x * (float)Math.Cos(angle) + y * (float)Math.Sin(angle));

-                }

-

-                public static Vector3 operator +(Vector3 a, Vector3 b)

-                {

-                        return new Vector3(a.X + b.X, a.Y + b.Y, a.Z + b.Z);

-                }

-

-                public static Vector3 operator -(Vector3 a, Vector3 b)

-                {

-                        return new Vector3(a.X - b.X, a.Y - b.Y, a.Z - b.Z);

-                }

-

-                public static Vector3 operator -(Vector3 a)

-                {

-                        return new Vector3(-a.X, -a.Y, -a.Z);

-                }

-

-                public static Vector3 operator *(float b, Vector3 a)

-                {

-                        return new Vector3(a.X * b, a.Y * b, a.Z * b);

-                }

-

-                public static Vector3 operator *(Vector3 a, float b)

-                {

-                        return new Vector3(a.X * b, a.Y * b, a.Z * b);

-                }

-

-                public static Vector3 operator *(Vector3 a, Vector3 b)

-                {

-                        return new Vector3(a.X * b.X, a.Y * b.Y, a.Z * b.Z);

-                }

-

-                public static Vector3 operator /(Vector3 a, float b)

-                {

-                        if (b == 0) throw new DivideByZeroException();

-                        return new Vector3(a.X / b, a.Y / b, a.Z / b);

-                }

-

-                public static Vector3 operator /(Vector3 a, Vector3 b)

-                {

-                        if (b.X == 0 || b.Y == 0 || b.Z == 0) throw new DivideByZeroException();

-                        return new Vector3(a.X / b.X, a.Y / b.Y, a.Z / b.Z);

-                }

-

-                public static bool operator ==(Vector3 a, Vector3 b)

-                {

-                        return a.X == b.X && a.Y == b.Y && a.Z == b.Z;

-                }

-

-                public static bool operator !=(Vector3 a, Vector3 b)

-                {

-                        return a.X != b.X || a.Y != b.Y || a.Z != b.Z;

-                }

-

-                public static explicit operator MonoXnaCompactMaths.Vector3(Vector3 a)

-                {

-                        return new MonoXnaCompactMaths.Vector3(a.X, a.Y, a.Z);

-                }

-

-                public override bool Equals(object obj)

-                {

-                        return object.Equals(this, obj);

-                }

-

-                public override int GetHashCode()

-                {

-                        return X.GetHashCode() & Y.GetHashCode() & Z.GetHashCode();

-                }

-

-                public override string ToString()

-                {

-                        return string.Format("{0}, {1}, {2}", X, Y, Z);

-                }

-        }

-}

+/*
+  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;
+
+namespace XnaDevRu.BulletX.LinearMath
+{
+        internal class Vector3 : QuadWord
+        {
+                public Vector3() { }
+
+                public Vector3(float x, float y, float z)
+                        : base(x, y, z) { }
+
+                public void SetInterpolate3(Vector3 a, Vector3 b, float c)
+                {
+                        float s = 1.0f - c;
+                        X = s * a.X + c * b.X;
+                        Y = s * a.Y + c * b.Y;
+                        Z = s * a.Z + c * b.Z;
+                }
+
+                public float LengthSquared()
+                {
+                        return Dot(this, this);
+                }
+
+                public float Length()
+                {
+                        return (float)Math.Sqrt(LengthSquared());
+                }
+
+                public int MinAxis()
+                {
+                        return X < Y ? (X < Z ? 0 : 2) : (Y < Z ? 1 : 2);
+                }
+
+                public int MaxAxis()
+                {
+                        return X < Y ? (Y < Z ? 2 : 1) : (X < Z ? 2 : 0);
+                }
+
+                public int FurthestAxis()
+                {
+                        return Absolute(this).MinAxis();
+                }
+
+                public int ClosestAxis()
+                {
+                        return Absolute(this).MaxAxis();
+                }
+
+                public Vector3 Rotate(Vector3 axis, float angle)
+                {
+                        Vector3 o = axis * Dot(axis, this);
+                        Vector3 x = this - o;
+                        Vector3 y = Cross(axis, this);
+
+                        return (o + x * (float)Math.Cos(angle) + y * (float)Math.Sin(angle));
+                }
+
+                public static Vector3 Lerp(Vector3 a, Vector3 b, float c)
+                {
+                        return new Vector3(
+                                a.X + (b.X - a.X) * c,
+                                a.Y + (b.Y - a.Y) * c,
+                                a.Z + (b.Z - a.Z) * c);
+                }
+
+                public static float Angle(Vector3 a, Vector3 b)
+                {
+                        float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared());
+                        if (s == 0) throw new DivideByZeroException();
+                        return (float)Math.Acos(Dot(a, b) / s);
+                }
+
+                public static Vector3 Absolute(Vector3 a)
+                {
+                        return new Vector3(
+                                Math.Abs(a.X),
+                                Math.Abs(a.Y),
+                                Math.Abs(a.Z));
+                }
+
+                public static Vector3 Normalize(Vector3 a)
+                {
+                        return a / a.Length();
+                }
+
+                public static Vector3 Cross(Vector3 a, Vector3 b)
+                {
+                        return new Vector3(
+                                a.Y * b.Z - a.Z * b.Y,
+                                a.Z * b.X - a.X * b.Z,
+                                a.X * b.Y - a.Y * b.X);
+                }
+
+                public static float Dot(Vector3 a, Vector3 b)
+                {
+                        return a.X * b.X + a.Y * b.Y + a.Z * b.Z;
+                }
+
+                public static float Triple(Vector3 a, Vector3 b, Vector3 c)
+                {
+                        return a.X * (b.Y * c.Z - b.Z * c.Y) +
+                                a.Y * (b.Z * c.X - b.X * c.Z) +
+                                a.Z * (b.X * c.Y - b.Y * c.X);
+                }
+
+                public static float Distance(Vector3 a, Vector3 b)
+                {
+                        return (b - a).Length();
+                }
+
+                public static float DistanceSquared(Vector3 a, Vector3 b)
+                {
+                        return (b - a).LengthSquared();
+                }
+
+                public static Vector3 Rotate(Vector3 a, Vector3 axis, float angle)
+                {
+                        Vector3 o = axis * Dot(axis, a);
+                        Vector3 x = a - o;
+                        Vector3 y = Cross(axis, a);
+
+                        return (o + x * (float)Math.Cos(angle) + y * (float)Math.Sin(angle));
+                }
+
+                public static Vector3 operator +(Vector3 a, Vector3 b)
+                {
+                        return new Vector3(a.X + b.X, a.Y + b.Y, a.Z + b.Z);
+                }
+
+                public static Vector3 operator -(Vector3 a, Vector3 b)
+                {
+                        return new Vector3(a.X - b.X, a.Y - b.Y, a.Z - b.Z);
+                }
+
+                public static Vector3 operator -(Vector3 a)
+                {
+                        return new Vector3(-a.X, -a.Y, -a.Z);
+                }
+
+                public static Vector3 operator *(float b, Vector3 a)
+                {
+                        return new Vector3(a.X * b, a.Y * b, a.Z * b);
+                }
+
+                public static Vector3 operator *(Vector3 a, float b)
+                {
+                        return new Vector3(a.X * b, a.Y * b, a.Z * b);
+                }
+
+                public static Vector3 operator *(Vector3 a, Vector3 b)
+                {
+                        return new Vector3(a.X * b.X, a.Y * b.Y, a.Z * b.Z);
+                }
+
+                public static Vector3 operator /(Vector3 a, float b)
+                {
+                        if (b == 0) throw new DivideByZeroException();
+                        return new Vector3(a.X / b, a.Y / b, a.Z / b);
+                }
+
+                public static Vector3 operator /(Vector3 a, Vector3 b)
+                {
+                        if (b.X == 0 || b.Y == 0 || b.Z == 0) throw new DivideByZeroException();
+                        return new Vector3(a.X / b.X, a.Y / b.Y, a.Z / b.Z);
+                }
+
+                public static bool operator ==(Vector3 a, Vector3 b)
+                {
+                        return a.X == b.X && a.Y == b.Y && a.Z == b.Z;
+                }
+
+                public static bool operator !=(Vector3 a, Vector3 b)
+                {
+                        return a.X != b.X || a.Y != b.Y || a.Z != b.Z;
+                }
+
+                public static explicit operator MonoXnaCompactMaths.Vector3(Vector3 a)
+                {
+                        return new MonoXnaCompactMaths.Vector3(a.X, a.Y, a.Z);
+                }
+
+                public override bool Equals(object obj)
+                {
+                        return object.Equals(this, obj);
+                }
+
+                public override int GetHashCode()
+                {
+                        return X.GetHashCode() & Y.GetHashCode() & Z.GetHashCode();
+                }
+
+                public override string ToString()
+                {
+                        return string.Format("{0}, {1}, {2}", X, Y, Z);
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs
index a64ce97..de98ccd 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs
@@ -1,110 +1,110 @@
-/*

-  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;

-

-namespace XnaDevRu.BulletX.LinearMath

-{

-        internal class Vector4 : Vector3

-        {

-                public Vector4() { }

-

-                public Vector4(float x, float y, float z, float w)

-                        : base(x, y, z) { W = w; }

-

-                public static Vector4 Absolute4(Vector4 a)

-                {

-                        return new Vector4(

-                                Math.Abs(a.X),

-                                Math.Abs(a.Y),

-                                Math.Abs(a.Z),

-                                Math.Abs(a.W));

-                }

-

-                public int MaxAxis4()

-                {

-                        int maxIndex = -1;

-                        float maxVal = -1e30f;

-                        if (X > maxVal)

-                        {

-                                maxIndex = 0;

-                                maxVal = X;

-                        }

-                        if (Y > maxVal)

-                        {

-                                maxIndex = 1;

-                                maxVal = Y;

-                        }

-                        if (Z > maxVal)

-                        {

-                                maxIndex = 2;

-                                maxVal = Z;

-                        }

-                        if (W > maxVal)

-                        {

-                                maxIndex = 3;

-                                maxVal = W;

-                        }

-

-                        return maxIndex;

-                }

-

-                public int MinAxis4()

-                {

-                        int minIndex = -1;

-                        float minVal = 1e30f;

-                        if (X < minVal)

-                        {

-                                minIndex = 0;

-                                minVal = X;

-                        }

-                        if (Y < minVal)

-                        {

-                                minIndex = 1;

-                                minVal = Y;

-                        }

-                        if (Z < minVal)

-                        {

-                                minIndex = 2;

-                                minVal = Z;

-                        }

-                        if (W < minVal)

-                        {

-                                minIndex = 3;

-                                minVal = W;

-                        }

-

-                        return minIndex;

-                }

-

-                public int ClosestAxis4()

-                {

-                        return Absolute4(this).MaxAxis4();

-                }

-

-                public static explicit operator MonoXnaCompactMaths.Vector4(Vector4 a)

-                {

-                        return new MonoXnaCompactMaths.Vector4(a.X, a.Y, a.Z, a.W);

-                }

-        }

-}

+/*
+  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;
+
+namespace XnaDevRu.BulletX.LinearMath
+{
+        internal class Vector4 : Vector3
+        {
+                public Vector4() { }
+
+                public Vector4(float x, float y, float z, float w)
+                        : base(x, y, z) { W = w; }
+
+                public static Vector4 Absolute4(Vector4 a)
+                {
+                        return new Vector4(
+                                Math.Abs(a.X),
+                                Math.Abs(a.Y),
+                                Math.Abs(a.Z),
+                                Math.Abs(a.W));
+                }
+
+                public int MaxAxis4()
+                {
+                        int maxIndex = -1;
+                        float maxVal = -1e30f;
+                        if (X > maxVal)
+                        {
+                                maxIndex = 0;
+                                maxVal = X;
+                        }
+                        if (Y > maxVal)
+                        {
+                                maxIndex = 1;
+                                maxVal = Y;
+                        }
+                        if (Z > maxVal)
+                        {
+                                maxIndex = 2;
+                                maxVal = Z;
+                        }
+                        if (W > maxVal)
+                        {
+                                maxIndex = 3;
+                                maxVal = W;
+                        }
+
+                        return maxIndex;
+                }
+
+                public int MinAxis4()
+                {
+                        int minIndex = -1;
+                        float minVal = 1e30f;
+                        if (X < minVal)
+                        {
+                                minIndex = 0;
+                                minVal = X;
+                        }
+                        if (Y < minVal)
+                        {
+                                minIndex = 1;
+                                minVal = Y;
+                        }
+                        if (Z < minVal)
+                        {
+                                minIndex = 2;
+                                minVal = Z;
+                        }
+                        if (W < minVal)
+                        {
+                                minIndex = 3;
+                                minVal = W;
+                        }
+
+                        return minIndex;
+                }
+
+                public int ClosestAxis4()
+                {
+                        return Absolute4(this).MaxAxis4();
+                }
+
+                public static explicit operator MonoXnaCompactMaths.Vector4(Vector4 a)
+                {
+                        return new MonoXnaCompactMaths.Vector4(a.X, a.Y, a.Z, a.W);
+                }
+        }
+}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj b/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj
index bed8d19..06b98bc 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj
@@ -1,168 +1,168 @@
-<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">

-  <PropertyGroup>

-    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>

-    <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>

-    <ProductVersion>8.0.50727</ProductVersion>

-    <SchemaVersion>2.0</SchemaVersion>

-    <ProjectGuid>{44270344-ACA7-4875-B585-81D5C06D0489}</ProjectGuid>

-    <OutputType>Library</OutputType>

-    <AppDesignerFolder>Properties</AppDesignerFolder>

-    <RootNamespace>XnaDevRu.BulletX</RootNamespace>

-    <AssemblyName>Modified.XnaDevRu.BulletX</AssemblyName>

-  </PropertyGroup>

-  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">

-    <DebugSymbols>true</DebugSymbols>

-    <DebugType>full</DebugType>

-    <Optimize>false</Optimize>

-    <OutputPath>bin\Debug\</OutputPath>

-    <DefineConstants>DEBUG;TRACE</DefineConstants>

-    <ErrorReport>prompt</ErrorReport>

-    <WarningLevel>4</WarningLevel>

-  </PropertyGroup>

-  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">

-    <DebugType>pdbonly</DebugType>

-    <Optimize>true</Optimize>

-    <OutputPath>bin\Release\</OutputPath>

-    <DefineConstants>TRACE</DefineConstants>

-    <ErrorReport>prompt</ErrorReport>

-    <WarningLevel>4</WarningLevel>

-  </PropertyGroup>

-  <ItemGroup>

-    <Reference Include="System" />

-  </ItemGroup>

-  <ItemGroup>

-    <Compile Include="BulletDebug.cs" />

-    <Compile Include="Collision\BroadphaseCollision\AxisSweep3.cs" />

-    <Compile Include="Collision\BroadphaseCollision\BroadphaseNativeTypes.cs" />

-    <Compile Include="Collision\BroadphaseCollision\BroadphasePair.cs" />

-    <Compile Include="Collision\BroadphaseCollision\BroadphaseProxy.cs" />

-    <Compile Include="Collision\BroadphaseCollision\CollisionAlgorithm.cs" />

-    <Compile Include="Collision\BroadphaseCollision\CollisionAlgorithmConstructionInfo.cs" />

-    <Compile Include="Collision\BroadphaseCollision\DispatcherInfo.cs" />

-    <Compile Include="Collision\BroadphaseCollision\IBroadphase.cs" />

-    <Compile Include="Collision\BroadphaseCollision\IDispatcher.cs" />

-    <Compile Include="Collision\BroadphaseCollision\IOverlapCallback.cs" />

-    <Compile Include="Collision\BroadphaseCollision\OverlappingPairCache.cs" />

-    <Compile Include="Collision\BroadphaseCollision\SimpleBroadphase.cs" />

-    <Compile Include="Collision\BroadphaseCollision\SimpleBroadphaseProxy.cs" />

-    <Compile Include="Collision\CollisionDispatch\BridgeTriangleRaycastCallback.cs" />

-    <Compile Include="Collision\CollisionDispatch\CollisionAlgorithmCreateFunc.cs" />

-    <Compile Include="Collision\CollisionDispatch\CollisionDispatcher.cs" />

-    <Compile Include="Collision\CollisionDispatch\CollisionObject.cs" />

-    <Compile Include="Collision\CollisionDispatch\CollisionPairCallback.cs" />

-    <Compile Include="Collision\CollisionDispatch\CollisionWorld.cs" />

-    <Compile Include="Collision\CollisionDispatch\CompoundCollisionAlgorithm.cs" />

-    <Compile Include="Collision\CollisionDispatch\ConvexConcaveCollisionAlgorithm.cs" />

-    <Compile Include="Collision\CollisionDispatch\ConvexConvexCollisionAlgorithm.cs" />

-    <Compile Include="Collision\CollisionDispatch\ConvexTriangleCallback.cs" />

-    <Compile Include="Collision\CollisionDispatch\EmptyAlgorithm.cs" />

-    <Compile Include="Collision\CollisionDispatch\ManifoldResult.cs" />

-    <Compile Include="Collision\CollisionDispatch\SimulationIslandManager.cs" />

-    <Compile Include="Collision\CollisionDispatch\SphereBoxCollisionAlgorithm.cs" />

-    <Compile Include="Collision\CollisionDispatch\SphereSphereCollisionAlgorithm.cs" />

-    <Compile Include="Collision\CollisionDispatch\SphereTriangleCollisionAlgorithm.cs" />

-    <Compile Include="Collision\CollisionDispatch\SphereTriangleDetector.cs" />

-    <Compile Include="Collision\CollisionDispatch\UnionFind.cs" />

-    <Compile Include="Collision\CollisionShapes\BoxShape.cs" />

-    <Compile Include="Collision\CollisionShapes\BUSimplex1to4.cs" />

-    <Compile Include="Collision\CollisionShapes\BvhTriangleMeshShape.cs" />

-    <Compile Include="Collision\CollisionShapes\CollisionShape.cs" />

-    <Compile Include="Collision\CollisionShapes\CompoundShape.cs" />

-    <Compile Include="Collision\CollisionShapes\ConcaveShape.cs" />

-    <Compile Include="Collision\CollisionShapes\ConeShape.cs" />

-    <Compile Include="Collision\CollisionShapes\ConvexHullShape.cs" />

-    <Compile Include="Collision\CollisionShapes\ConvexShape.cs" />

-    <Compile Include="Collision\CollisionShapes\ConvexTriangleMeshShape.cs" />

-    <Compile Include="Collision\CollisionShapes\CylinderShape.cs" />

-    <Compile Include="Collision\CollisionShapes\CylinderShapeX.cs" />

-    <Compile Include="Collision\CollisionShapes\CylinderShapeZ.cs" />

-    <Compile Include="Collision\CollisionShapes\EmptyShape.cs" />

-    <Compile Include="Collision\CollisionShapes\FilteredCallback.cs" />

-    <Compile Include="Collision\CollisionShapes\InternalTriangleIndexCallback.cs" />

-    <Compile Include="Collision\CollisionShapes\LocalSupportVertexCallback.cs" />

-    <Compile Include="Collision\CollisionShapes\MinkowskiSumShape.cs" />

-    <Compile Include="Collision\CollisionShapes\MultiSphereShape.cs" />

-    <Compile Include="Collision\CollisionShapes\NodeOverlapCallback.cs" />

-    <Compile Include="Collision\CollisionShapes\OptimizedBvh.cs" />

-    <Compile Include="Collision\CollisionShapes\OptimizedBvhNode.cs" />

-    <Compile Include="Collision\CollisionShapes\PolyhedralConvexShape.cs" />

-    <Compile Include="Collision\CollisionShapes\SphereShape.cs" />

-    <Compile Include="Collision\CollisionShapes\StaticPlaneShape.cs" />

-    <Compile Include="Collision\CollisionShapes\StridingMeshInterface.cs" />

-    <Compile Include="Collision\CollisionShapes\SupportVertexCallback.cs" />

-    <Compile Include="Collision\CollisionShapes\TriangleBuffer.cs" />

-    <Compile Include="Collision\CollisionShapes\TriangleCallback.cs" />

-    <Compile Include="Collision\CollisionShapes\TriangleIndexVertexArray.cs" />

-    <Compile Include="Collision\CollisionShapes\TriangleMesh.cs" />

-    <Compile Include="Collision\CollisionShapes\TriangleMeshShape.cs" />

-    <Compile Include="Collision\CollisionShapes\TriangleShape.cs" />

-    <Compile Include="Collision\NarrowPhaseCollision\ContinuousConvexCollision.cs" />

-    <Compile Include="Collision\NarrowPhaseCollision\ConvexCast.cs" />

-    <Compile Include="Collision\NarrowPhaseCollision\DiscreteCollisionDetectorInterface.cs" />

-    <Compile Include="Collision\NarrowPhaseCollision\GjkConvexCast.cs" />

-    <Compile Include="Collision\NarrowPhaseCollision\GjkEpa.cs" />

-    <Compile Include="Collision\NarrowPhaseCollision\GjkEpaPenetrationDepthSolver.cs" />

-    <Compile Include="Collision\NarrowPhaseCollision\GjkEpaSolver.cs" />

-    <Compile Include="Collision\NarrowPhaseCollision\GjkPairDetector.cs" />

-    <Compile Include="Collision\NarrowPhaseCollision\IConvexPenetrationDepthSolver.cs" />

-    <Compile Include="Collision\NarrowPhaseCollision\ISimplexSolver.cs" />

-    <Compile Include="Collision\NarrowPhaseCollision\ManifoldPoint.cs" />

-    <Compile Include="Collision\NarrowPhaseCollision\MinkowskiPenetrationDepthSolver.cs" />

-    <Compile Include="Collision\NarrowPhaseCollision\PersistentManifold.cs" />

-    <Compile Include="Collision\NarrowPhaseCollision\PointCollector.cs" />

-    <Compile Include="Collision\NarrowPhaseCollision\SubsimplexConvexCast.cs" />

-    <Compile Include="Collision\NarrowPhaseCollision\TriangleRaycastCallback.cs" />

-    <Compile Include="Collision\NarrowPhaseCollision\VoronoiSimplexSolver.cs" />

-    <Compile Include="Dynamics\ConstraintSolver\ContactConstraint.cs" />

-    <Compile Include="Dynamics\ConstraintSolver\ContactSolverInfo.cs" />

-    <Compile Include="Dynamics\ConstraintSolver\Generic6DofConstraint.cs" />

-    <Compile Include="Dynamics\ConstraintSolver\HingeConstraint.cs" />

-    <Compile Include="Dynamics\ConstraintSolver\IConstraintSolver.cs" />

-    <Compile Include="Dynamics\ConstraintSolver\JacobianEntry.cs" />

-    <Compile Include="Dynamics\ConstraintSolver\Point2PointConstraint.cs" />

-    <Compile Include="Dynamics\ConstraintSolver\SequentialImpulseConstraintSolver.cs" />

-    <Compile Include="Dynamics\ConstraintSolver\Solve2LinearConstraint.cs" />

-    <Compile Include="Dynamics\ConstraintSolver\SolverBody.cs" />

-    <Compile Include="Dynamics\ConstraintSolver\SolverConstraint.cs" />

-    <Compile Include="Dynamics\ConstraintSolver\TypedConstraint.cs" />

-    <Compile Include="Dynamics\DiscreteDynamicsWorld.cs" />

-    <Compile Include="Dynamics\DynamicsWorld.cs" />

-    <Compile Include="Dynamics\RigidBody.cs" />

-    <Compile Include="Dynamics\SimpleDynamicsWorld.cs" />

-    <Compile Include="Dynamics\Vehicle\RaycastVehicle.cs" />

-    <Compile Include="Dynamics\Vehicle\VehicleRaycaster.cs" />

-    <Compile Include="Dynamics\Vehicle\WheelInfo.cs" />

-    <Compile Include="Exceptions\BulletException.cs" />

-    <Compile Include="IDebugDraw.cs" />

-    <Compile Include="LinearMath\DefaultMotionState.cs" />

-    <Compile Include="LinearMath\MathHelper.cs" />

-    <Compile Include="LinearMath\MatrixOperations.cs" />

-    <Compile Include="LinearMath\MotionState.cs" />

-    <Compile Include="LinearMath\QuadWord.cs" />

-    <Compile Include="LinearMath\Quaternion.cs" />

-    <Compile Include="LinearMath\TransformUtil.cs" />

-    <Compile Include="LinearMath\Vector3.cs" />

-    <Compile Include="LinearMath\Vector4.cs" />

-    <Compile Include="Properties\AssemblyInfo.cs" />

-  </ItemGroup>

-  <ItemGroup>

-    <ProjectReference Include="..\MonoXnaCompactMaths\MonoXnaCompactMaths.csproj">

-      <Project>{121147BC-B06B-406C-84E9-907F268CF0EB}</Project>

-      <Name>MonoXnaCompactMaths</Name>

-    </ProjectReference>

-  </ItemGroup>

-  <ItemGroup>

-    <None Include="BulletX.snk" />

-  </ItemGroup>

-  <ItemGroup>

-    <Folder Include="Content\" />

-  </ItemGroup>

-  <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />

-  <!-- To modify your build process, add your task inside one of the targets below and uncomment it. 

-       Other similar extension points exist, see Microsoft.Common.targets.

-  <Target Name="BeforeBuild">

-  </Target>

-  <Target Name="AfterBuild">

-  </Target>

-  -->

+<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <PropertyGroup>
+    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
+    <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
+    <ProductVersion>8.0.50727</ProductVersion>
+    <SchemaVersion>2.0</SchemaVersion>
+    <ProjectGuid>{44270344-ACA7-4875-B585-81D5C06D0489}</ProjectGuid>
+    <OutputType>Library</OutputType>
+    <AppDesignerFolder>Properties</AppDesignerFolder>
+    <RootNamespace>XnaDevRu.BulletX</RootNamespace>
+    <AssemblyName>Modified.XnaDevRu.BulletX</AssemblyName>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
+    <DebugSymbols>true</DebugSymbols>
+    <DebugType>full</DebugType>
+    <Optimize>false</Optimize>
+    <OutputPath>bin\Debug\</OutputPath>
+    <DefineConstants>DEBUG;TRACE</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
+    <DebugType>pdbonly</DebugType>
+    <Optimize>true</Optimize>
+    <OutputPath>bin\Release\</OutputPath>
+    <DefineConstants>TRACE</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+  </PropertyGroup>
+  <ItemGroup>
+    <Reference Include="System" />
+  </ItemGroup>
+  <ItemGroup>
+    <Compile Include="BulletDebug.cs" />
+    <Compile Include="Collision\BroadphaseCollision\AxisSweep3.cs" />
+    <Compile Include="Collision\BroadphaseCollision\BroadphaseNativeTypes.cs" />
+    <Compile Include="Collision\BroadphaseCollision\BroadphasePair.cs" />
+    <Compile Include="Collision\BroadphaseCollision\BroadphaseProxy.cs" />
+    <Compile Include="Collision\BroadphaseCollision\CollisionAlgorithm.cs" />
+    <Compile Include="Collision\BroadphaseCollision\CollisionAlgorithmConstructionInfo.cs" />
+    <Compile Include="Collision\BroadphaseCollision\DispatcherInfo.cs" />
+    <Compile Include="Collision\BroadphaseCollision\IBroadphase.cs" />
+    <Compile Include="Collision\BroadphaseCollision\IDispatcher.cs" />
+    <Compile Include="Collision\BroadphaseCollision\IOverlapCallback.cs" />
+    <Compile Include="Collision\BroadphaseCollision\OverlappingPairCache.cs" />
+    <Compile Include="Collision\BroadphaseCollision\SimpleBroadphase.cs" />
+    <Compile Include="Collision\BroadphaseCollision\SimpleBroadphaseProxy.cs" />
+    <Compile Include="Collision\CollisionDispatch\BridgeTriangleRaycastCallback.cs" />
+    <Compile Include="Collision\CollisionDispatch\CollisionAlgorithmCreateFunc.cs" />
+    <Compile Include="Collision\CollisionDispatch\CollisionDispatcher.cs" />
+    <Compile Include="Collision\CollisionDispatch\CollisionObject.cs" />
+    <Compile Include="Collision\CollisionDispatch\CollisionPairCallback.cs" />
+    <Compile Include="Collision\CollisionDispatch\CollisionWorld.cs" />
+    <Compile Include="Collision\CollisionDispatch\CompoundCollisionAlgorithm.cs" />
+    <Compile Include="Collision\CollisionDispatch\ConvexConcaveCollisionAlgorithm.cs" />
+    <Compile Include="Collision\CollisionDispatch\ConvexConvexCollisionAlgorithm.cs" />
+    <Compile Include="Collision\CollisionDispatch\ConvexTriangleCallback.cs" />
+    <Compile Include="Collision\CollisionDispatch\EmptyAlgorithm.cs" />
+    <Compile Include="Collision\CollisionDispatch\ManifoldResult.cs" />
+    <Compile Include="Collision\CollisionDispatch\SimulationIslandManager.cs" />
+    <Compile Include="Collision\CollisionDispatch\SphereBoxCollisionAlgorithm.cs" />
+    <Compile Include="Collision\CollisionDispatch\SphereSphereCollisionAlgorithm.cs" />
+    <Compile Include="Collision\CollisionDispatch\SphereTriangleCollisionAlgorithm.cs" />
+    <Compile Include="Collision\CollisionDispatch\SphereTriangleDetector.cs" />
+    <Compile Include="Collision\CollisionDispatch\UnionFind.cs" />
+    <Compile Include="Collision\CollisionShapes\BoxShape.cs" />
+    <Compile Include="Collision\CollisionShapes\BUSimplex1to4.cs" />
+    <Compile Include="Collision\CollisionShapes\BvhTriangleMeshShape.cs" />
+    <Compile Include="Collision\CollisionShapes\CollisionShape.cs" />
+    <Compile Include="Collision\CollisionShapes\CompoundShape.cs" />
+    <Compile Include="Collision\CollisionShapes\ConcaveShape.cs" />
+    <Compile Include="Collision\CollisionShapes\ConeShape.cs" />
+    <Compile Include="Collision\CollisionShapes\ConvexHullShape.cs" />
+    <Compile Include="Collision\CollisionShapes\ConvexShape.cs" />
+    <Compile Include="Collision\CollisionShapes\ConvexTriangleMeshShape.cs" />
+    <Compile Include="Collision\CollisionShapes\CylinderShape.cs" />
+    <Compile Include="Collision\CollisionShapes\CylinderShapeX.cs" />
+    <Compile Include="Collision\CollisionShapes\CylinderShapeZ.cs" />
+    <Compile Include="Collision\CollisionShapes\EmptyShape.cs" />
+    <Compile Include="Collision\CollisionShapes\FilteredCallback.cs" />
+    <Compile Include="Collision\CollisionShapes\InternalTriangleIndexCallback.cs" />
+    <Compile Include="Collision\CollisionShapes\LocalSupportVertexCallback.cs" />
+    <Compile Include="Collision\CollisionShapes\MinkowskiSumShape.cs" />
+    <Compile Include="Collision\CollisionShapes\MultiSphereShape.cs" />
+    <Compile Include="Collision\CollisionShapes\NodeOverlapCallback.cs" />
+    <Compile Include="Collision\CollisionShapes\OptimizedBvh.cs" />
+    <Compile Include="Collision\CollisionShapes\OptimizedBvhNode.cs" />
+    <Compile Include="Collision\CollisionShapes\PolyhedralConvexShape.cs" />
+    <Compile Include="Collision\CollisionShapes\SphereShape.cs" />
+    <Compile Include="Collision\CollisionShapes\StaticPlaneShape.cs" />
+    <Compile Include="Collision\CollisionShapes\StridingMeshInterface.cs" />
+    <Compile Include="Collision\CollisionShapes\SupportVertexCallback.cs" />
+    <Compile Include="Collision\CollisionShapes\TriangleBuffer.cs" />
+    <Compile Include="Collision\CollisionShapes\TriangleCallback.cs" />
+    <Compile Include="Collision\CollisionShapes\TriangleIndexVertexArray.cs" />
+    <Compile Include="Collision\CollisionShapes\TriangleMesh.cs" />
+    <Compile Include="Collision\CollisionShapes\TriangleMeshShape.cs" />
+    <Compile Include="Collision\CollisionShapes\TriangleShape.cs" />
+    <Compile Include="Collision\NarrowPhaseCollision\ContinuousConvexCollision.cs" />
+    <Compile Include="Collision\NarrowPhaseCollision\ConvexCast.cs" />
+    <Compile Include="Collision\NarrowPhaseCollision\DiscreteCollisionDetectorInterface.cs" />
+    <Compile Include="Collision\NarrowPhaseCollision\GjkConvexCast.cs" />
+    <Compile Include="Collision\NarrowPhaseCollision\GjkEpa.cs" />
+    <Compile Include="Collision\NarrowPhaseCollision\GjkEpaPenetrationDepthSolver.cs" />
+    <Compile Include="Collision\NarrowPhaseCollision\GjkEpaSolver.cs" />
+    <Compile Include="Collision\NarrowPhaseCollision\GjkPairDetector.cs" />
+    <Compile Include="Collision\NarrowPhaseCollision\IConvexPenetrationDepthSolver.cs" />
+    <Compile Include="Collision\NarrowPhaseCollision\ISimplexSolver.cs" />
+    <Compile Include="Collision\NarrowPhaseCollision\ManifoldPoint.cs" />
+    <Compile Include="Collision\NarrowPhaseCollision\MinkowskiPenetrationDepthSolver.cs" />
+    <Compile Include="Collision\NarrowPhaseCollision\PersistentManifold.cs" />
+    <Compile Include="Collision\NarrowPhaseCollision\PointCollector.cs" />
+    <Compile Include="Collision\NarrowPhaseCollision\SubsimplexConvexCast.cs" />
+    <Compile Include="Collision\NarrowPhaseCollision\TriangleRaycastCallback.cs" />
+    <Compile Include="Collision\NarrowPhaseCollision\VoronoiSimplexSolver.cs" />
+    <Compile Include="Dynamics\ConstraintSolver\ContactConstraint.cs" />
+    <Compile Include="Dynamics\ConstraintSolver\ContactSolverInfo.cs" />
+    <Compile Include="Dynamics\ConstraintSolver\Generic6DofConstraint.cs" />
+    <Compile Include="Dynamics\ConstraintSolver\HingeConstraint.cs" />
+    <Compile Include="Dynamics\ConstraintSolver\IConstraintSolver.cs" />
+    <Compile Include="Dynamics\ConstraintSolver\JacobianEntry.cs" />
+    <Compile Include="Dynamics\ConstraintSolver\Point2PointConstraint.cs" />
+    <Compile Include="Dynamics\ConstraintSolver\SequentialImpulseConstraintSolver.cs" />
+    <Compile Include="Dynamics\ConstraintSolver\Solve2LinearConstraint.cs" />
+    <Compile Include="Dynamics\ConstraintSolver\SolverBody.cs" />
+    <Compile Include="Dynamics\ConstraintSolver\SolverConstraint.cs" />
+    <Compile Include="Dynamics\ConstraintSolver\TypedConstraint.cs" />
+    <Compile Include="Dynamics\DiscreteDynamicsWorld.cs" />
+    <Compile Include="Dynamics\DynamicsWorld.cs" />
+    <Compile Include="Dynamics\RigidBody.cs" />
+    <Compile Include="Dynamics\SimpleDynamicsWorld.cs" />
+    <Compile Include="Dynamics\Vehicle\RaycastVehicle.cs" />
+    <Compile Include="Dynamics\Vehicle\VehicleRaycaster.cs" />
+    <Compile Include="Dynamics\Vehicle\WheelInfo.cs" />
+    <Compile Include="Exceptions\BulletException.cs" />
+    <Compile Include="IDebugDraw.cs" />
+    <Compile Include="LinearMath\DefaultMotionState.cs" />
+    <Compile Include="LinearMath\MathHelper.cs" />
+    <Compile Include="LinearMath\MatrixOperations.cs" />
+    <Compile Include="LinearMath\MotionState.cs" />
+    <Compile Include="LinearMath\QuadWord.cs" />
+    <Compile Include="LinearMath\Quaternion.cs" />
+    <Compile Include="LinearMath\TransformUtil.cs" />
+    <Compile Include="LinearMath\Vector3.cs" />
+    <Compile Include="LinearMath\Vector4.cs" />
+    <Compile Include="Properties\AssemblyInfo.cs" />
+  </ItemGroup>
+  <ItemGroup>
+    <ProjectReference Include="..\MonoXnaCompactMaths\MonoXnaCompactMaths.csproj">
+      <Project>{121147BC-B06B-406C-84E9-907F268CF0EB}</Project>
+      <Name>MonoXnaCompactMaths</Name>
+    </ProjectReference>
+  </ItemGroup>
+  <ItemGroup>
+    <None Include="BulletX.snk" />
+  </ItemGroup>
+  <ItemGroup>
+    <Folder Include="Content\" />
+  </ItemGroup>
+  <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />
+  <!-- To modify your build process, add your task inside one of the targets below and uncomment it. 
+       Other similar extension points exist, see Microsoft.Common.targets.
+  <Target Name="BeforeBuild">
+  </Target>
+  <Target Name="AfterBuild">
+  </Target>
+  -->
 </Project>
\ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs
index efc8fd6..03d1e3e 100644
--- a/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs
@@ -1,35 +1,35 @@
-using System.Reflection;

-using System.Runtime.CompilerServices;

-using System.Runtime.InteropServices;

-

-// La información general sobre un ensamblado se controla mediante el siguiente 

-// conjunto de atributos. Cambie estos atributos para modificar la información

-// asociada con un ensamblado.

-[assembly: AssemblyTitle("Modified.XnaDevRu.BulletX")]

-[assembly: AssemblyDescription("Modified Bullet for XNA")]

-[assembly: AssemblyConfiguration("")]

-[assembly: AssemblyCompany("XNADev.ru")]

-[assembly: AssemblyProduct("Bullet for XNA")]

-[assembly: AssemblyCopyright("Copyright © 2007 XNADev.ru")]

-[assembly: AssemblyTrademark("")]

-[assembly: AssemblyCulture("")]

-

-// Si establece ComVisible como false hace que los tipos de este ensamblado no sean visibles 

-// a los componentes COM. Si necesita obtener acceso a un tipo en este ensamblado desde 

-// COM, establezca el atributo ComVisible como true en este tipo.

-[assembly: ComVisible(false)]

-

-// El siguiente GUID sirve como identificador de la biblioteca de tipos si este proyecto se expone a COM

-[assembly: Guid("a6148224-0271-4cd8-9d0a-5d4955cd2b2c")]

-

-// La información de versión de un ensamblado consta de los cuatro valores siguientes:

-//

-//      Versión principal

-//      Versión secundaria 

-//      Número de versión de compilación

-//      Revisión

-//

-// Puede especificar todos los valores o puede establecer como valores predeterminados los números de revisión y generación 

-// mediante el asterisco ('*'), como se muestra a continuación:

-[assembly: AssemblyVersion("1.0.0.0")]

-[assembly: AssemblyFileVersion("1.0.0.0")]

+using System.Reflection;
+using System.Runtime.CompilerServices;
+using System.Runtime.InteropServices;
+
+// La información general sobre un ensamblado se controla mediante el siguiente 
+// conjunto de atributos. Cambie estos atributos para modificar la información
+// asociada con un ensamblado.
+[assembly: AssemblyTitle("Modified.XnaDevRu.BulletX")]
+[assembly: AssemblyDescription("Modified Bullet for XNA")]
+[assembly: AssemblyConfiguration("")]
+[assembly: AssemblyCompany("XNADev.ru")]
+[assembly: AssemblyProduct("Bullet for XNA")]
+[assembly: AssemblyCopyright("Copyright © 2007 XNADev.ru")]
+[assembly: AssemblyTrademark("")]
+[assembly: AssemblyCulture("")]
+
+// Si establece ComVisible como false hace que los tipos de este ensamblado no sean visibles 
+// a los componentes COM. Si necesita obtener acceso a un tipo en este ensamblado desde 
+// COM, establezca el atributo ComVisible como true en este tipo.
+[assembly: ComVisible(false)]
+
+// El siguiente GUID sirve como identificador de la biblioteca de tipos si este proyecto se expone a COM
+[assembly: Guid("a6148224-0271-4cd8-9d0a-5d4955cd2b2c")]
+
+// La información de versión de un ensamblado consta de los cuatro valores siguientes:
+//
+//      Versión principal
+//      Versión secundaria 
+//      Número de versión de compilación
+//      Revisión
+//
+// Puede especificar todos los valores o puede establecer como valores predeterminados los números de revisión y generación 
+// mediante el asterisco ('*'), como se muestra a continuación:
+[assembly: AssemblyVersion("1.0.0.0")]
+[assembly: AssemblyFileVersion("1.0.0.0")]
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs
index a104d89..22842ca 100644
--- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs
+++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs
@@ -1,683 +1,683 @@
-#region License

-/*

-MIT License

-Copyright © 2006 The Mono.Xna Team

-

-All rights reserved.

-

-Permission is hereby granted, free of charge, to any person obtaining a copy

-of this software and associated documentation files (the "Software"), to deal

-in the Software without restriction, including without limitation the rights

-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell

-copies of the Software, and to permit persons to whom the Software is

-furnished to do so, subject to the following conditions:

-

-The above copyright notice and this permission notice shall be included in all

-copies or substantial portions of the Software.

-

-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR

-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,

-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE

-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER

-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,

-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE

-SOFTWARE.

-*/

-#endregion License

-

-using System;

-using System.ComponentModel;

-using System.Runtime.InteropServices;

-

-namespace MonoXnaCompactMaths

-{

-    [Serializable]

-    [StructLayout(LayoutKind.Sequential)]

-        //[TypeConverter(typeof(MatrixConverter))]

-    public struct Matrix : IEquatable<Matrix>

-    {

-        #region Public Constructors

-        

-        public Matrix(float m11, float m12, float m13, float m14, float m21, float m22, float m23, float m24, float m31,

-                      float m32, float m33, float m34, float m41, float m42, float m43, float m44)

-        {

-            this.M11 = m11;

-            this.M12 = m12;

-            this.M13 = m13;

-            this.M14 = m14;

-            this.M21 = m21;

-            this.M22 = m22;

-            this.M23 = m23;

-            this.M24 = m24;

-            this.M31 = m31;

-            this.M32 = m32;

-            this.M33 = m33;

-            this.M34 = m34;

-            this.M41 = m41;

-            this.M42 = m42;

-            this.M43 = m43;

-            this.M44 = m44;

-        }

-

-        #endregion Public Constructors

-

-

-        #region Public Fields

-

-        public float M11;

-        public float M12;

-        public float M13;

-        public float M14;

-        public float M21;

-        public float M22;

-        public float M23;

-        public float M24;

-        public float M31;

-        public float M32;

-        public float M33;

-        public float M34;

-        public float M41;

-        public float M42;

-        public float M43;

-        public float M44;

-

-        #endregion Public Fields

-

-

-        #region Private Members

-        private static Matrix identity = new Matrix(1f, 0f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 0f, 1f);

-        #endregion Private Members

-

-

-        #region Public Properties

-        

-        public Vector3 Backward

-        {

-            get

-            {

-                return new Vector3(this.M31, this.M32, this.M33);

-            }

-            set

-            {

-                this.M31 = value.X;

-                this.M32 = value.Y;

-                this.M33 = value.Z;

-            }

-        }

-

-        

-        public Vector3 Down

-        {

-            get

-            {

-                return new Vector3(-this.M21, -this.M22, -this.M23);

-            }

-            set

-            {

-                this.M21 = -value.X;

-                this.M22 = -value.Y;

-                this.M23 = -value.Z;

-            }

-        }

-

-        

-        public Vector3 Forward

-        {

-            get

-            {

-                return new Vector3(-this.M31, -this.M32, -this.M33);

-            }

-            set

-            {

-                this.M31 = -value.X;

-                this.M32 = -value.Y;

-                this.M33 = -value.Z;

-            }

-        }

-

-        

-        public static Matrix Identity

-        {

-            get { return identity; }

-        }

-

-        

-        public Vector3 Left

-        {

-            get

-            {

-                return new Vector3(-this.M11, -this.M12, -this.M13);

-            }

-            set

-            {

-                this.M11 = -value.X;

-                this.M12 = -value.Y;

-                this.M13 = -value.Z;

-            }

-        }

-

-        

-        public Vector3 Right

-        {

-            get

-            {

-                return new Vector3(this.M11, this.M12, this.M13);

-            }

-            set

-            {

-                this.M11 = value.X;

-                this.M12 = value.Y;

-                this.M13 = value.Z;

-            }

-        }

-

-        

-        public Vector3 Translation

-        {

-            get

-            {

-                return new Vector3(this.M41, this.M42, this.M43);

-            }

-            set

-            {

-                this.M41 = value.X;

-                this.M42 = value.Y;

-                this.M43 = value.Z;

-            }

-        }

-

-        

-        public Vector3 Up

-        {

-            get

-            {

-                return new Vector3(this.M21, this.M22, this.M23);

-            }

-            set

-            {

-                this.M21 = value.X;

-                this.M22 = value.Y;

-                this.M23 = value.Z;

-            }

-        }

-        #endregion Public Properties

-

-

-        #region Public Methods

-

-        public static Matrix Add(Matrix matrix1, Matrix matrix2)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void Add(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-        

-        public static Matrix CreateBillboard(Vector3 objectPosition, Vector3 cameraPosition,

-            Vector3 cameraUpVector, Nullable<Vector3> cameraForwardVector)

-        {

-            throw new NotImplementedException();

-        }

-

-        

-        public static void CreateBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition,

-            ref Vector3 cameraUpVector, Vector3? cameraForwardVector, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-        

-        public static Matrix CreateConstrainedBillboard(Vector3 objectPosition, Vector3 cameraPosition,

-            Vector3 rotateAxis, Nullable<Vector3> cameraForwardVector, Nullable<Vector3> objectForwardVector)

-        {

-            throw new NotImplementedException();

-        }

-

-        

-        public static void CreateConstrainedBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition,

-            ref Vector3 rotateAxis, Vector3? cameraForwardVector, Vector3? objectForwardVector, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix CreateFromAxisAngle(Vector3 axis, float angle)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void CreateFromAxisAngle(ref Vector3 axis, float angle, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix CreateFromQuaternion(Quaternion quaternion)

-        {

-            //---

-            //http://lists.ximian.com/pipermail/mono-patches/2006-December/084667.html

-            float xx = quaternion.X * quaternion.X;

-            float xy = quaternion.X * quaternion.Y;

-            float xw = quaternion.X * quaternion.W;

-            float yy = quaternion.Y * quaternion.Y;

-            float yw = quaternion.Y * quaternion.W;

-            float yz = quaternion.Y * quaternion.Z;

-            float zx = quaternion.Z * quaternion.X;

-            float zw = quaternion.Z * quaternion.W;

-            float zz = quaternion.Z * quaternion.Z;

-            return new Matrix(1f - (2f * (yy + zz)), 2f * (xy + zw), 2f * (zx - yw), 0f, 2f * (xy - zw),

-                            1f - (2f * (zz + xx)), 2f * (yz + xw), 0f, 2f * (zx + yw), 2f * (yz - xw),

-                            1f - (2f * (yy + xx)), 0f, 0f, 0f, 0f, 1f);

-            //throw new NotImplementedException();

-        }

-

-

-        public static void CreateFromQuaternion(ref Quaternion quaternion, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix CreateLookAt(Vector3 cameraPosition, Vector3 cameraTarget, Vector3 cameraUpVector)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void CreateLookAt(ref Vector3 cameraPosition, ref Vector3 cameraTarget, ref Vector3 cameraUpVector, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix CreateOrthographicOffCenter(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane)

-        {

-            throw new NotImplementedException();

-        }

-

-        

-        public static void CreateOrthographicOffCenter(float left, float right, float bottom, float top,

-            float zNearPlane, float zFarPlane, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix CreatePerspective(float width, float height, float zNearPlane, float zFarPlane)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void CreatePerspective(float width, float height, float zNearPlane, float zFarPlane, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float zNearPlane, float zFarPlane)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float nearPlaneDistance, float farPlaneDistance, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix CreatePerspectiveOffCenter(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void CreatePerspectiveOffCenter(float left, float right, float bottom, float top, float nearPlaneDistance, float farPlaneDistance, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix CreateRotationX(float radians)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void CreateRotationX(float radians, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix CreateRotationY(float radians)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void CreateRotationY(float radians, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix CreateRotationZ(float radians)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void CreateRotationZ(float radians, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix CreateScale(float scale)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void CreateScale(float scale, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix CreateScale(float xScale, float yScale, float zScale)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void CreateScale(float xScale, float yScale, float zScale, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix CreateScale(Vector3 scales)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void CreateScale(ref Vector3 scales, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix CreateTranslation(float xPosition, float yPosition, float zPosition)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void CreateTranslation(ref Vector3 position, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix CreateTranslation(Vector3 position)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void CreateTranslation(float xPosition, float yPosition, float zPosition, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public float Determinant()

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix Divide(Matrix matrix1, Matrix matrix2)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void Divide(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix Divide(Matrix matrix1, float divider)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void Divide(ref Matrix matrix1, float divider, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public bool Equals(Matrix other)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public override bool Equals(object obj)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public override int GetHashCode()

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix Invert(Matrix matrix)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void Invert(ref Matrix matrix, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix Lerp(Matrix matrix1, Matrix matrix2, float amount)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void Lerp(ref Matrix matrix1, ref Matrix matrix2, float amount, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-        public static Matrix Multiply(Matrix matrix1, Matrix matrix2)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void Multiply(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix Multiply(Matrix matrix1, float factor)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void Multiply(ref Matrix matrix1, float factor, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix Negate(Matrix matrix)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void Negate(ref Matrix matrix, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix operator +(Matrix matrix1, Matrix matrix2)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix operator /(Matrix matrix1, Matrix matrix2)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix operator /(Matrix matrix1, float divider)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static bool operator ==(Matrix matrix1, Matrix matrix2)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static bool operator !=(Matrix matrix1, Matrix matrix2)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix operator *(Matrix matrix1, Matrix matrix2)

-        {

-            //---

-            float[, ] arrayMatrix1 = new float[4, 4];

-            float[, ] arrayMatrix2 = new float[4, 4];

-            float[, ] arrayMatrixProduct = new float[4, 4];

-            arrayMatrix1[0, 0] = matrix1.M11; arrayMatrix1[0, 1] = matrix1.M12; arrayMatrix1[0, 2] = matrix1.M13; arrayMatrix1[0, 3] = matrix1.M14;

-            arrayMatrix1[1, 0] = matrix1.M21; arrayMatrix1[1, 1] = matrix1.M22; arrayMatrix1[1, 2] = matrix1.M23; arrayMatrix1[1, 3] = matrix1.M24;

-            arrayMatrix1[2, 0] = matrix1.M31; arrayMatrix1[2, 1] = matrix1.M32; arrayMatrix1[2, 2] = matrix1.M33; arrayMatrix1[2, 3] = matrix1.M34;

-            arrayMatrix1[3, 0] = matrix1.M41; arrayMatrix1[3, 1] = matrix1.M42; arrayMatrix1[3, 2] = matrix1.M43; arrayMatrix1[3, 3] = matrix1.M44;

-

-            arrayMatrix2[0, 0] = matrix2.M11; arrayMatrix2[0, 1] = matrix2.M12; arrayMatrix2[0, 2] = matrix2.M13; arrayMatrix2[0, 3] = matrix2.M14;

-            arrayMatrix2[1, 0] = matrix2.M21; arrayMatrix2[1, 1] = matrix2.M22; arrayMatrix2[1, 2] = matrix2.M23; arrayMatrix2[1, 3] = matrix2.M24;

-            arrayMatrix2[2, 0] = matrix2.M31; arrayMatrix2[2, 1] = matrix2.M32; arrayMatrix2[2, 2] = matrix2.M33; arrayMatrix2[2, 3] = matrix2.M34;

-            arrayMatrix2[3, 0] = matrix2.M41; arrayMatrix2[3, 1] = matrix2.M42; arrayMatrix2[3, 2] = matrix2.M43; arrayMatrix2[3, 3] = matrix2.M44;

-

-            int n = 4;

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

-            {

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

-                {

-                    // (AB)[i,j] = Sum(k=0; k < 4; k++) { A[i,k] * B[k, j] }

-                    for (int k = 0; k < n; k++)

-                    {

-                        arrayMatrixProduct[i, j] += arrayMatrix1[i, k] * arrayMatrix2[k, j];

-                    }

-                }

-            }

-            return new Matrix(  arrayMatrixProduct[0, 0], arrayMatrixProduct[0, 1], arrayMatrixProduct[0, 2], arrayMatrixProduct[0, 3],

-                                arrayMatrixProduct[1, 0], arrayMatrixProduct[1, 1], arrayMatrixProduct[1, 2], arrayMatrixProduct[1, 3],

-                                arrayMatrixProduct[2, 0], arrayMatrixProduct[2, 1], arrayMatrixProduct[2, 2], arrayMatrixProduct[2, 3],

-                                arrayMatrixProduct[3, 1], arrayMatrixProduct[3, 1], arrayMatrixProduct[3, 2], arrayMatrixProduct[3, 3]);

-            //---

-            //throw new NotImplementedException();

-        }

-

-

-        public static Matrix operator *(Matrix matrix, float scaleFactor)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix operator -(Matrix matrix1, Matrix matrix2)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix operator -(Matrix matrix1)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix Subtract(Matrix matrix1, Matrix matrix2)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void Subtract(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public override string ToString()

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Matrix Transpose(Matrix matrix)

-        {

-            //---

-            return new Matrix(  matrix.M11, matrix.M21, matrix.M31, matrix.M41,

-                                matrix.M12, matrix.M22, matrix.M32, matrix.M42,

-                                matrix.M13, matrix.M23, matrix.M33, matrix.M43,

-                                matrix.M14, matrix.M24, matrix.M34, matrix.M44);

-            //---

-            //throw new NotImplementedException();

-        }

-

-        

-        public static void Transpose(ref Matrix matrix, out Matrix result)

-        {

-            throw new NotImplementedException();

-        }

-        #endregion Public Methods

-    }

-}

+#region License
+/*
+MIT License
+Copyright © 2006 The Mono.Xna Team
+
+All rights reserved.
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+#endregion License
+
+using System;
+using System.ComponentModel;
+using System.Runtime.InteropServices;
+
+namespace MonoXnaCompactMaths
+{
+    [Serializable]
+    [StructLayout(LayoutKind.Sequential)]
+        //[TypeConverter(typeof(MatrixConverter))]
+    public struct Matrix : IEquatable<Matrix>
+    {
+        #region Public Constructors
+        
+        public Matrix(float m11, float m12, float m13, float m14, float m21, float m22, float m23, float m24, float m31,
+                      float m32, float m33, float m34, float m41, float m42, float m43, float m44)
+        {
+            this.M11 = m11;
+            this.M12 = m12;
+            this.M13 = m13;
+            this.M14 = m14;
+            this.M21 = m21;
+            this.M22 = m22;
+            this.M23 = m23;
+            this.M24 = m24;
+            this.M31 = m31;
+            this.M32 = m32;
+            this.M33 = m33;
+            this.M34 = m34;
+            this.M41 = m41;
+            this.M42 = m42;
+            this.M43 = m43;
+            this.M44 = m44;
+        }
+
+        #endregion Public Constructors
+
+
+        #region Public Fields
+
+        public float M11;
+        public float M12;
+        public float M13;
+        public float M14;
+        public float M21;
+        public float M22;
+        public float M23;
+        public float M24;
+        public float M31;
+        public float M32;
+        public float M33;
+        public float M34;
+        public float M41;
+        public float M42;
+        public float M43;
+        public float M44;
+
+        #endregion Public Fields
+
+
+        #region Private Members
+        private static Matrix identity = new Matrix(1f, 0f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 0f, 1f);
+        #endregion Private Members
+
+
+        #region Public Properties
+        
+        public Vector3 Backward
+        {
+            get
+            {
+                return new Vector3(this.M31, this.M32, this.M33);
+            }
+            set
+            {
+                this.M31 = value.X;
+                this.M32 = value.Y;
+                this.M33 = value.Z;
+            }
+        }
+
+        
+        public Vector3 Down
+        {
+            get
+            {
+                return new Vector3(-this.M21, -this.M22, -this.M23);
+            }
+            set
+            {
+                this.M21 = -value.X;
+                this.M22 = -value.Y;
+                this.M23 = -value.Z;
+            }
+        }
+
+        
+        public Vector3 Forward
+        {
+            get
+            {
+                return new Vector3(-this.M31, -this.M32, -this.M33);
+            }
+            set
+            {
+                this.M31 = -value.X;
+                this.M32 = -value.Y;
+                this.M33 = -value.Z;
+            }
+        }
+
+        
+        public static Matrix Identity
+        {
+            get { return identity; }
+        }
+
+        
+        public Vector3 Left
+        {
+            get
+            {
+                return new Vector3(-this.M11, -this.M12, -this.M13);
+            }
+            set
+            {
+                this.M11 = -value.X;
+                this.M12 = -value.Y;
+                this.M13 = -value.Z;
+            }
+        }
+
+        
+        public Vector3 Right
+        {
+            get
+            {
+                return new Vector3(this.M11, this.M12, this.M13);
+            }
+            set
+            {
+                this.M11 = value.X;
+                this.M12 = value.Y;
+                this.M13 = value.Z;
+            }
+        }
+
+        
+        public Vector3 Translation
+        {
+            get
+            {
+                return new Vector3(this.M41, this.M42, this.M43);
+            }
+            set
+            {
+                this.M41 = value.X;
+                this.M42 = value.Y;
+                this.M43 = value.Z;
+            }
+        }
+
+        
+        public Vector3 Up
+        {
+            get
+            {
+                return new Vector3(this.M21, this.M22, this.M23);
+            }
+            set
+            {
+                this.M21 = value.X;
+                this.M22 = value.Y;
+                this.M23 = value.Z;
+            }
+        }
+        #endregion Public Properties
+
+
+        #region Public Methods
+
+        public static Matrix Add(Matrix matrix1, Matrix matrix2)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void Add(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+        
+        public static Matrix CreateBillboard(Vector3 objectPosition, Vector3 cameraPosition,
+            Vector3 cameraUpVector, Nullable<Vector3> cameraForwardVector)
+        {
+            throw new NotImplementedException();
+        }
+
+        
+        public static void CreateBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition,
+            ref Vector3 cameraUpVector, Vector3? cameraForwardVector, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+        
+        public static Matrix CreateConstrainedBillboard(Vector3 objectPosition, Vector3 cameraPosition,
+            Vector3 rotateAxis, Nullable<Vector3> cameraForwardVector, Nullable<Vector3> objectForwardVector)
+        {
+            throw new NotImplementedException();
+        }
+
+        
+        public static void CreateConstrainedBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition,
+            ref Vector3 rotateAxis, Vector3? cameraForwardVector, Vector3? objectForwardVector, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix CreateFromAxisAngle(Vector3 axis, float angle)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void CreateFromAxisAngle(ref Vector3 axis, float angle, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix CreateFromQuaternion(Quaternion quaternion)
+        {
+            //---
+            //http://lists.ximian.com/pipermail/mono-patches/2006-December/084667.html
+            float xx = quaternion.X * quaternion.X;
+            float xy = quaternion.X * quaternion.Y;
+            float xw = quaternion.X * quaternion.W;
+            float yy = quaternion.Y * quaternion.Y;
+            float yw = quaternion.Y * quaternion.W;
+            float yz = quaternion.Y * quaternion.Z;
+            float zx = quaternion.Z * quaternion.X;
+            float zw = quaternion.Z * quaternion.W;
+            float zz = quaternion.Z * quaternion.Z;
+            return new Matrix(1f - (2f * (yy + zz)), 2f * (xy + zw), 2f * (zx - yw), 0f, 2f * (xy - zw),
+                            1f - (2f * (zz + xx)), 2f * (yz + xw), 0f, 2f * (zx + yw), 2f * (yz - xw),
+                            1f - (2f * (yy + xx)), 0f, 0f, 0f, 0f, 1f);
+            //throw new NotImplementedException();
+        }
+
+
+        public static void CreateFromQuaternion(ref Quaternion quaternion, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix CreateLookAt(Vector3 cameraPosition, Vector3 cameraTarget, Vector3 cameraUpVector)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void CreateLookAt(ref Vector3 cameraPosition, ref Vector3 cameraTarget, ref Vector3 cameraUpVector, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix CreateOrthographicOffCenter(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane)
+        {
+            throw new NotImplementedException();
+        }
+
+        
+        public static void CreateOrthographicOffCenter(float left, float right, float bottom, float top,
+            float zNearPlane, float zFarPlane, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix CreatePerspective(float width, float height, float zNearPlane, float zFarPlane)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void CreatePerspective(float width, float height, float zNearPlane, float zFarPlane, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float zNearPlane, float zFarPlane)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float nearPlaneDistance, float farPlaneDistance, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix CreatePerspectiveOffCenter(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void CreatePerspectiveOffCenter(float left, float right, float bottom, float top, float nearPlaneDistance, float farPlaneDistance, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix CreateRotationX(float radians)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void CreateRotationX(float radians, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix CreateRotationY(float radians)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void CreateRotationY(float radians, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix CreateRotationZ(float radians)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void CreateRotationZ(float radians, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix CreateScale(float scale)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void CreateScale(float scale, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix CreateScale(float xScale, float yScale, float zScale)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void CreateScale(float xScale, float yScale, float zScale, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix CreateScale(Vector3 scales)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void CreateScale(ref Vector3 scales, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix CreateTranslation(float xPosition, float yPosition, float zPosition)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void CreateTranslation(ref Vector3 position, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix CreateTranslation(Vector3 position)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void CreateTranslation(float xPosition, float yPosition, float zPosition, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public float Determinant()
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix Divide(Matrix matrix1, Matrix matrix2)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void Divide(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix Divide(Matrix matrix1, float divider)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void Divide(ref Matrix matrix1, float divider, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public bool Equals(Matrix other)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public override bool Equals(object obj)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public override int GetHashCode()
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix Invert(Matrix matrix)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void Invert(ref Matrix matrix, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix Lerp(Matrix matrix1, Matrix matrix2, float amount)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void Lerp(ref Matrix matrix1, ref Matrix matrix2, float amount, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+        public static Matrix Multiply(Matrix matrix1, Matrix matrix2)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void Multiply(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix Multiply(Matrix matrix1, float factor)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void Multiply(ref Matrix matrix1, float factor, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix Negate(Matrix matrix)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void Negate(ref Matrix matrix, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix operator +(Matrix matrix1, Matrix matrix2)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix operator /(Matrix matrix1, Matrix matrix2)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix operator /(Matrix matrix1, float divider)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static bool operator ==(Matrix matrix1, Matrix matrix2)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static bool operator !=(Matrix matrix1, Matrix matrix2)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix operator *(Matrix matrix1, Matrix matrix2)
+        {
+            //---
+            float[, ] arrayMatrix1 = new float[4, 4];
+            float[, ] arrayMatrix2 = new float[4, 4];
+            float[, ] arrayMatrixProduct = new float[4, 4];
+            arrayMatrix1[0, 0] = matrix1.M11; arrayMatrix1[0, 1] = matrix1.M12; arrayMatrix1[0, 2] = matrix1.M13; arrayMatrix1[0, 3] = matrix1.M14;
+            arrayMatrix1[1, 0] = matrix1.M21; arrayMatrix1[1, 1] = matrix1.M22; arrayMatrix1[1, 2] = matrix1.M23; arrayMatrix1[1, 3] = matrix1.M24;
+            arrayMatrix1[2, 0] = matrix1.M31; arrayMatrix1[2, 1] = matrix1.M32; arrayMatrix1[2, 2] = matrix1.M33; arrayMatrix1[2, 3] = matrix1.M34;
+            arrayMatrix1[3, 0] = matrix1.M41; arrayMatrix1[3, 1] = matrix1.M42; arrayMatrix1[3, 2] = matrix1.M43; arrayMatrix1[3, 3] = matrix1.M44;
+
+            arrayMatrix2[0, 0] = matrix2.M11; arrayMatrix2[0, 1] = matrix2.M12; arrayMatrix2[0, 2] = matrix2.M13; arrayMatrix2[0, 3] = matrix2.M14;
+            arrayMatrix2[1, 0] = matrix2.M21; arrayMatrix2[1, 1] = matrix2.M22; arrayMatrix2[1, 2] = matrix2.M23; arrayMatrix2[1, 3] = matrix2.M24;
+            arrayMatrix2[2, 0] = matrix2.M31; arrayMatrix2[2, 1] = matrix2.M32; arrayMatrix2[2, 2] = matrix2.M33; arrayMatrix2[2, 3] = matrix2.M34;
+            arrayMatrix2[3, 0] = matrix2.M41; arrayMatrix2[3, 1] = matrix2.M42; arrayMatrix2[3, 2] = matrix2.M43; arrayMatrix2[3, 3] = matrix2.M44;
+
+            int n = 4;
+            for (int i = 0; i < n; i++)
+            {
+                for (int j = 0; j < n; j++)
+                {
+                    // (AB)[i,j] = Sum(k=0; k < 4; k++) { A[i,k] * B[k, j] }
+                    for (int k = 0; k < n; k++)
+                    {
+                        arrayMatrixProduct[i, j] += arrayMatrix1[i, k] * arrayMatrix2[k, j];
+                    }
+                }
+            }
+            return new Matrix(  arrayMatrixProduct[0, 0], arrayMatrixProduct[0, 1], arrayMatrixProduct[0, 2], arrayMatrixProduct[0, 3],
+                                arrayMatrixProduct[1, 0], arrayMatrixProduct[1, 1], arrayMatrixProduct[1, 2], arrayMatrixProduct[1, 3],
+                                arrayMatrixProduct[2, 0], arrayMatrixProduct[2, 1], arrayMatrixProduct[2, 2], arrayMatrixProduct[2, 3],
+                                arrayMatrixProduct[3, 1], arrayMatrixProduct[3, 1], arrayMatrixProduct[3, 2], arrayMatrixProduct[3, 3]);
+            //---
+            //throw new NotImplementedException();
+        }
+
+
+        public static Matrix operator *(Matrix matrix, float scaleFactor)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix operator -(Matrix matrix1, Matrix matrix2)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix operator -(Matrix matrix1)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix Subtract(Matrix matrix1, Matrix matrix2)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void Subtract(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public override string ToString()
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Matrix Transpose(Matrix matrix)
+        {
+            //---
+            return new Matrix(  matrix.M11, matrix.M21, matrix.M31, matrix.M41,
+                                matrix.M12, matrix.M22, matrix.M32, matrix.M42,
+                                matrix.M13, matrix.M23, matrix.M33, matrix.M43,
+                                matrix.M14, matrix.M24, matrix.M34, matrix.M44);
+            //---
+            //throw new NotImplementedException();
+        }
+
+        
+        public static void Transpose(ref Matrix matrix, out Matrix result)
+        {
+            throw new NotImplementedException();
+        }
+        #endregion Public Methods
+    }
+}
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/MonoXnaCompactMaths.csproj b/libraries/ModifiedBulletX/MonoXnaCompactMaths/MonoXnaCompactMaths.csproj
index c3ec4dd..79c5263 100644
--- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/MonoXnaCompactMaths.csproj
+++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/MonoXnaCompactMaths.csproj
@@ -1,51 +1,51 @@
-<?xml version="1.0" encoding="utf-8"?>

-<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">

-  <PropertyGroup>

-    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>

-    <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>

-    <ProductVersion>8.0.50727</ProductVersion>

-    <SchemaVersion>2.0</SchemaVersion>

-    <ProjectGuid>{121147BC-B06B-406C-84E9-907F268CF0EB}</ProjectGuid>

-    <OutputType>Library</OutputType>

-    <AppDesignerFolder>Properties</AppDesignerFolder>

-    <RootNamespace>MonoXnaCompactMaths</RootNamespace>

-    <AssemblyName>MonoXnaCompactMaths</AssemblyName>

-    <StartupObject>

-    </StartupObject>

-  </PropertyGroup>

-  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">

-    <DebugSymbols>true</DebugSymbols>

-    <DebugType>full</DebugType>

-    <Optimize>false</Optimize>

-    <OutputPath>bin\Debug\</OutputPath>

-    <DefineConstants>DEBUG;TRACE</DefineConstants>

-    <ErrorReport>prompt</ErrorReport>

-    <WarningLevel>4</WarningLevel>

-  </PropertyGroup>

-  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">

-    <DebugType>pdbonly</DebugType>

-    <Optimize>true</Optimize>

-    <OutputPath>bin\Release\</OutputPath>

-    <DefineConstants>TRACE</DefineConstants>

-    <ErrorReport>prompt</ErrorReport>

-    <WarningLevel>4</WarningLevel>

-  </PropertyGroup>

-  <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />

-  <!-- To modify your build process, add your task inside one of the targets below and uncomment it. 

-       Other similar extension points exist, see Microsoft.Common.targets.

-  <Target Name="BeforeBuild">

-  </Target>

-  <Target Name="AfterBuild">

-  </Target>

-  -->

-  <ItemGroup>

-    <Compile Include="Matrix.cs" />

-    <Compile Include="Properties\AssemblyInfo.cs" />

-    <Compile Include="Quaternion.cs" />

-    <Compile Include="Vector3.cs" />

-    <Compile Include="Vector4.cs" />

-  </ItemGroup>

-  <ItemGroup>

-    <Reference Include="System" />

-  </ItemGroup>

+<?xml version="1.0" encoding="utf-8"?>
+<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
+  <PropertyGroup>
+    <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
+    <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
+    <ProductVersion>8.0.50727</ProductVersion>
+    <SchemaVersion>2.0</SchemaVersion>
+    <ProjectGuid>{121147BC-B06B-406C-84E9-907F268CF0EB}</ProjectGuid>
+    <OutputType>Library</OutputType>
+    <AppDesignerFolder>Properties</AppDesignerFolder>
+    <RootNamespace>MonoXnaCompactMaths</RootNamespace>
+    <AssemblyName>MonoXnaCompactMaths</AssemblyName>
+    <StartupObject>
+    </StartupObject>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
+    <DebugSymbols>true</DebugSymbols>
+    <DebugType>full</DebugType>
+    <Optimize>false</Optimize>
+    <OutputPath>bin\Debug\</OutputPath>
+    <DefineConstants>DEBUG;TRACE</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+  </PropertyGroup>
+  <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
+    <DebugType>pdbonly</DebugType>
+    <Optimize>true</Optimize>
+    <OutputPath>bin\Release\</OutputPath>
+    <DefineConstants>TRACE</DefineConstants>
+    <ErrorReport>prompt</ErrorReport>
+    <WarningLevel>4</WarningLevel>
+  </PropertyGroup>
+  <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />
+  <!-- To modify your build process, add your task inside one of the targets below and uncomment it. 
+       Other similar extension points exist, see Microsoft.Common.targets.
+  <Target Name="BeforeBuild">
+  </Target>
+  <Target Name="AfterBuild">
+  </Target>
+  -->
+  <ItemGroup>
+    <Compile Include="Matrix.cs" />
+    <Compile Include="Properties\AssemblyInfo.cs" />
+    <Compile Include="Quaternion.cs" />
+    <Compile Include="Vector3.cs" />
+    <Compile Include="Vector4.cs" />
+  </ItemGroup>
+  <ItemGroup>
+    <Reference Include="System" />
+  </ItemGroup>
 </Project>
\ No newline at end of file
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Properties/AssemblyInfo.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Properties/AssemblyInfo.cs
index 748b94a..366f664 100644
--- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Properties/AssemblyInfo.cs
+++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Properties/AssemblyInfo.cs
@@ -1,33 +1,33 @@
-using System.Reflection;

-using System.Runtime.CompilerServices;

-using System.Runtime.InteropServices;

-

-// La información general sobre un ensamblado se controla mediante el siguiente 

-// conjunto de atributos. Cambie estos atributos para modificar la información

-// asociada con un ensamblado.

-[assembly: AssemblyTitle("MonoXnaCompactMaths")]

-[assembly: AssemblyDescription("MonoXnaCompactMaths (Vector3, Vector4, Quaternion and Matrix)")]

-[assembly: AssemblyConfiguration("")]

-[assembly: AssemblyCompany("")]

-[assembly: AssemblyProduct("")]

-[assembly: AssemblyCopyright("Copyright 2006 - 2007 The Mono.Xna Team")]

-[assembly: AssemblyTrademark("")]

-[assembly: AssemblyCulture("")]

-

-// Si establece ComVisible como false hace que los tipos de este ensamblado no sean visibles 

-// a los componentes COM. Si necesita obtener acceso a un tipo en este ensamblado desde 

-// COM, establezca el atributo ComVisible como true en este tipo.

-[assembly: ComVisible(false)]

-

-// El siguiente GUID sirve como identificador de la biblioteca de tipos si este proyecto se expone a COM

-[assembly: Guid("79f9dfd1-60f8-44b8-ab08-6f6e341976b7")]

-

-// La información de versión de un ensamblado consta de los cuatro valores siguientes:

-//

-//      Versión principal

-//      Versión secundaria 

-//      Número de versión de compilación

-//      Revisión

-//

-[assembly: AssemblyVersion("1.0.0.0")]

-[assembly: AssemblyFileVersion("1.0.0.0")]

+using System.Reflection;
+using System.Runtime.CompilerServices;
+using System.Runtime.InteropServices;
+
+// La información general sobre un ensamblado se controla mediante el siguiente 
+// conjunto de atributos. Cambie estos atributos para modificar la información
+// asociada con un ensamblado.
+[assembly: AssemblyTitle("MonoXnaCompactMaths")]
+[assembly: AssemblyDescription("MonoXnaCompactMaths (Vector3, Vector4, Quaternion and Matrix)")]
+[assembly: AssemblyConfiguration("")]
+[assembly: AssemblyCompany("")]
+[assembly: AssemblyProduct("")]
+[assembly: AssemblyCopyright("Copyright 2006 - 2007 The Mono.Xna Team")]
+[assembly: AssemblyTrademark("")]
+[assembly: AssemblyCulture("")]
+
+// Si establece ComVisible como false hace que los tipos de este ensamblado no sean visibles 
+// a los componentes COM. Si necesita obtener acceso a un tipo en este ensamblado desde 
+// COM, establezca el atributo ComVisible como true en este tipo.
+[assembly: ComVisible(false)]
+
+// El siguiente GUID sirve como identificador de la biblioteca de tipos si este proyecto se expone a COM
+[assembly: Guid("79f9dfd1-60f8-44b8-ab08-6f6e341976b7")]
+
+// La información de versión de un ensamblado consta de los cuatro valores siguientes:
+//
+//      Versión principal
+//      Versión secundaria 
+//      Número de versión de compilación
+//      Revisión
+//
+[assembly: AssemblyVersion("1.0.0.0")]
+[assembly: AssemblyFileVersion("1.0.0.0")]
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs
index 6ed1443..b4f1873 100644
--- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs
+++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs
@@ -1,346 +1,346 @@
-#region License

-/*

-MIT License

-Copyright © 2006 The Mono.Xna Team

-

-All rights reserved.

-

-Permission is hereby granted, free of charge, to any person obtaining a copy

-of this software and associated documentation files (the "Software"), to deal

-in the Software without restriction, including without limitation the rights

-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell

-copies of the Software, and to permit persons to whom the Software is

-furnished to do so, subject to the following conditions:

-

-The above copyright notice and this permission notice shall be included in all

-copies or substantial portions of the Software.

-

-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR

-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,

-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE

-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER

-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,

-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE

-SOFTWARE.

-*/

-#endregion License

-

-using System;

-using System.ComponentModel;

-

-namespace MonoXnaCompactMaths

-{

-    [Serializable]

-    //[TypeConverter(typeof(QuaternionConverter))]

-    public struct Quaternion : IEquatable<Quaternion>

-    {

-        public float X;

-        public float Y;

-        public float Z;

-        public float W;

-        static Quaternion identity = new Quaternion(0, 0, 0, 1);

-

-        

-        public Quaternion(float x, float y, float z, float w)

-        {

-            this.X = x;

-            this.Y = y;

-            this.Z = z;

-            this.W = w;

-        }

-        

-        

-        public Quaternion(Vector3 vectorPart, float scalarPart)

-        {

-            this.X = vectorPart.X;

-            this.Y = vectorPart.Y;

-            this.Z = vectorPart.Z;

-            this.W = scalarPart;

-        }

-

-        public static Quaternion Identity

-        {

-            get{ return identity; }

-        }

-

-

-        public static Quaternion Add(Quaternion quaternion1, Quaternion quaternion2)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void Add(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void CreateFromAxisAngle(ref Vector3 axis, float angle, out Quaternion result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Quaternion CreateFromRotationMatrix(Matrix matrix)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void CreateFromRotationMatrix(ref Matrix matrix, out Quaternion result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Quaternion Divide(Quaternion quaternion1, Quaternion quaternion2)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void Divide(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static float Dot(Quaternion quaternion1, Quaternion quaternion2)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void Dot(ref Quaternion quaternion1, ref Quaternion quaternion2, out float result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public override bool Equals(object obj)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public bool Equals(Quaternion other)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public override int GetHashCode()

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Quaternion Inverse(Quaternion quaternion)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void Inverse(ref Quaternion quaternion, out Quaternion result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public float Length()

-        {

-            //---

-            return (float)Math.Sqrt(Math.Pow(this.W, 2.0) + Math.Pow(this.X, 2.0) + Math.Pow(this.Y, 2.0) + Math.Pow(this.Z, 2.0));

-            //---

-            //throw new NotImplementedException();

-        }

-

-

-        public float LengthSquared()

-        {

-            //---

-            return (float)(Math.Pow(this.W, 2.0) + Math.Pow(this.X, 2.0) + Math.Pow(this.Y, 2.0) + Math.Pow(this.Z, 2.0));

-            //---

-            //throw new NotImplementedException();

-        }

-

-

-        public static Quaternion Lerp(Quaternion quaternion1, Quaternion quaternion2, float amount)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void Lerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, float amount)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void Slerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Quaternion Subtract(Quaternion quaternion1, Quaternion quaternion2)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void Subtract(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Quaternion Multiply(Quaternion quaternion1, Quaternion quaternion2)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Quaternion Multiply(Quaternion quaternion1, float scaleFactor)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void Multiply(ref Quaternion quaternion1, float scaleFactor, out Quaternion result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void Multiply(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Quaternion Negate(Quaternion quaternion)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static void Negate(ref Quaternion quaternion, out Quaternion result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public void Normalize()

-        {

-            //---

-            this = Normalize(this);

-            //---

-            //throw new NotImplementedException();

-        }

-

-

-        public static Quaternion Normalize(Quaternion quaternion)

-        {

-            //---

-            return quaternion / quaternion.Length();

-            //---

-            //throw new NotImplementedException();

-        }

-

-

-        public static void Normalize(ref Quaternion quaternion, out Quaternion result)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Quaternion operator +(Quaternion quaternion1, Quaternion quaternion2)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Quaternion operator /(Quaternion quaternion1, Quaternion quaternion2)

-        {

-            throw new NotImplementedException();

-        }

-        public static Quaternion operator /(Quaternion quaternion, float factor)

-        {

-            quaternion.W /= factor;

-            quaternion.X /= factor;

-            quaternion.Y /= factor;

-            quaternion.Z /= factor;

-            return quaternion;

-        }

-

-        public static bool operator ==(Quaternion quaternion1, Quaternion quaternion2)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static bool operator !=(Quaternion quaternion1, Quaternion quaternion2)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Quaternion operator *(Quaternion quaternion1, Quaternion quaternion2)

-        {

-            //---

-            //Grassmann product

-            Quaternion quaternionProduct = new Quaternion();

-

-            quaternionProduct.W = quaternion1.W * quaternion2.W - quaternion1.X * quaternion2.X - quaternion1.Y * quaternion2.Y - quaternion1.Z * quaternion2.Z;

-            quaternionProduct.X = quaternion1.W * quaternion2.X + quaternion1.X * quaternion2.W + quaternion1.Y * quaternion2.Z - quaternion1.Z * quaternion2.Y;

-            quaternionProduct.Y = quaternion1.W * quaternion2.Y - quaternion1.X * quaternion2.Z + quaternion1.Y * quaternion2.W + quaternion1.Z * quaternion2.X;

-            quaternionProduct.Z = quaternion1.W * quaternion2.Z + quaternion1.X * quaternion2.Y - quaternion1.Y * quaternion2.X + quaternion1.Z * quaternion2.W;

-            return quaternionProduct;

-            //---

-            //throw new NotImplementedException();

-        }

-

-

-        public static Quaternion operator *(Quaternion quaternion1, float scaleFactor)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Quaternion operator -(Quaternion quaternion1, Quaternion quaternion2)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public static Quaternion operator -(Quaternion quaternion)

-        {

-            throw new NotImplementedException();

-        }

-

-

-        public override string ToString()

-        {

-            throw new NotImplementedException();

-        }

-

-        private static void Conjugate(ref Quaternion quaternion, out Quaternion result)

-        {

-            throw new NotImplementedException();

-        }

-    }

-}

+#region License
+/*
+MIT License
+Copyright © 2006 The Mono.Xna Team
+
+All rights reserved.
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+#endregion License
+
+using System;
+using System.ComponentModel;
+
+namespace MonoXnaCompactMaths
+{
+    [Serializable]
+    //[TypeConverter(typeof(QuaternionConverter))]
+    public struct Quaternion : IEquatable<Quaternion>
+    {
+        public float X;
+        public float Y;
+        public float Z;
+        public float W;
+        static Quaternion identity = new Quaternion(0, 0, 0, 1);
+
+        
+        public Quaternion(float x, float y, float z, float w)
+        {
+            this.X = x;
+            this.Y = y;
+            this.Z = z;
+            this.W = w;
+        }
+        
+        
+        public Quaternion(Vector3 vectorPart, float scalarPart)
+        {
+            this.X = vectorPart.X;
+            this.Y = vectorPart.Y;
+            this.Z = vectorPart.Z;
+            this.W = scalarPart;
+        }
+
+        public static Quaternion Identity
+        {
+            get{ return identity; }
+        }
+
+
+        public static Quaternion Add(Quaternion quaternion1, Quaternion quaternion2)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void Add(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void CreateFromAxisAngle(ref Vector3 axis, float angle, out Quaternion result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Quaternion CreateFromRotationMatrix(Matrix matrix)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void CreateFromRotationMatrix(ref Matrix matrix, out Quaternion result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Quaternion Divide(Quaternion quaternion1, Quaternion quaternion2)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void Divide(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static float Dot(Quaternion quaternion1, Quaternion quaternion2)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void Dot(ref Quaternion quaternion1, ref Quaternion quaternion2, out float result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public override bool Equals(object obj)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public bool Equals(Quaternion other)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public override int GetHashCode()
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Quaternion Inverse(Quaternion quaternion)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void Inverse(ref Quaternion quaternion, out Quaternion result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public float Length()
+        {
+            //---
+            return (float)Math.Sqrt(Math.Pow(this.W, 2.0) + Math.Pow(this.X, 2.0) + Math.Pow(this.Y, 2.0) + Math.Pow(this.Z, 2.0));
+            //---
+            //throw new NotImplementedException();
+        }
+
+
+        public float LengthSquared()
+        {
+            //---
+            return (float)(Math.Pow(this.W, 2.0) + Math.Pow(this.X, 2.0) + Math.Pow(this.Y, 2.0) + Math.Pow(this.Z, 2.0));
+            //---
+            //throw new NotImplementedException();
+        }
+
+
+        public static Quaternion Lerp(Quaternion quaternion1, Quaternion quaternion2, float amount)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void Lerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, float amount)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void Slerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Quaternion Subtract(Quaternion quaternion1, Quaternion quaternion2)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void Subtract(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Quaternion Multiply(Quaternion quaternion1, Quaternion quaternion2)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Quaternion Multiply(Quaternion quaternion1, float scaleFactor)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void Multiply(ref Quaternion quaternion1, float scaleFactor, out Quaternion result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void Multiply(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Quaternion Negate(Quaternion quaternion)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static void Negate(ref Quaternion quaternion, out Quaternion result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public void Normalize()
+        {
+            //---
+            this = Normalize(this);
+            //---
+            //throw new NotImplementedException();
+        }
+
+
+        public static Quaternion Normalize(Quaternion quaternion)
+        {
+            //---
+            return quaternion / quaternion.Length();
+            //---
+            //throw new NotImplementedException();
+        }
+
+
+        public static void Normalize(ref Quaternion quaternion, out Quaternion result)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Quaternion operator +(Quaternion quaternion1, Quaternion quaternion2)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Quaternion operator /(Quaternion quaternion1, Quaternion quaternion2)
+        {
+            throw new NotImplementedException();
+        }
+        public static Quaternion operator /(Quaternion quaternion, float factor)
+        {
+            quaternion.W /= factor;
+            quaternion.X /= factor;
+            quaternion.Y /= factor;
+            quaternion.Z /= factor;
+            return quaternion;
+        }
+
+        public static bool operator ==(Quaternion quaternion1, Quaternion quaternion2)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static bool operator !=(Quaternion quaternion1, Quaternion quaternion2)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Quaternion operator *(Quaternion quaternion1, Quaternion quaternion2)
+        {
+            //---
+            //Grassmann product
+            Quaternion quaternionProduct = new Quaternion();
+
+            quaternionProduct.W = quaternion1.W * quaternion2.W - quaternion1.X * quaternion2.X - quaternion1.Y * quaternion2.Y - quaternion1.Z * quaternion2.Z;
+            quaternionProduct.X = quaternion1.W * quaternion2.X + quaternion1.X * quaternion2.W + quaternion1.Y * quaternion2.Z - quaternion1.Z * quaternion2.Y;
+            quaternionProduct.Y = quaternion1.W * quaternion2.Y - quaternion1.X * quaternion2.Z + quaternion1.Y * quaternion2.W + quaternion1.Z * quaternion2.X;
+            quaternionProduct.Z = quaternion1.W * quaternion2.Z + quaternion1.X * quaternion2.Y - quaternion1.Y * quaternion2.X + quaternion1.Z * quaternion2.W;
+            return quaternionProduct;
+            //---
+            //throw new NotImplementedException();
+        }
+
+
+        public static Quaternion operator *(Quaternion quaternion1, float scaleFactor)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Quaternion operator -(Quaternion quaternion1, Quaternion quaternion2)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public static Quaternion operator -(Quaternion quaternion)
+        {
+            throw new NotImplementedException();
+        }
+
+
+        public override string ToString()
+        {
+            throw new NotImplementedException();
+        }
+
+        private static void Conjugate(ref Quaternion quaternion, out Quaternion result)
+        {
+            throw new NotImplementedException();
+        }
+    }
+}
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector3.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector3.cs
index fd4bcd2..f246415 100644
--- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector3.cs
+++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector3.cs
@@ -1,620 +1,620 @@
-#region License

-/*

-MIT License

-Copyright © 2006 The Mono.Xna Team

-

-All rights reserved.

-

-Authors:

- * Alan McGovern

-

-Permission is hereby granted, free of charge, to any person obtaining a copy

-of this software and associated documentation files (the "Software"), to deal

-in the Software without restriction, including without limitation the rights

-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell

-copies of the Software, and to permit persons to whom the Software is

-furnished to do so, subject to the following conditions:

-

-The above copyright notice and this permission notice shall be included in all

-copies or substantial portions of the Software.

-

-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR

-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,

-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE

-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER

-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,

-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE

-SOFTWARE.

-*/

-#endregion License

-

-using System;

-using System.ComponentModel;

-using System.Text;

-using System.Runtime.InteropServices;

-

-namespace MonoXnaCompactMaths

-{

-    [Serializable]

-    [StructLayout(LayoutKind.Sequential)]

-    //[TypeConverter(typeof(Vector3Converter))]

-    public struct Vector3 : IEquatable<Vector3>

-    {

-        #region Private Fields

-

-        private static  Vector3 zero = new Vector3(0f, 0f, 0f);

-        private static  Vector3 one = new Vector3(1f, 1f, 1f);

-        private static  Vector3 unitX = new Vector3(1f, 0f, 0f);

-        private static  Vector3 unitY = new Vector3(0f, 1f, 0f);

-        private static  Vector3 unitZ = new Vector3(0f, 0f, 1f);

-        private static  Vector3 up = new Vector3(0f, 1f, 0f);

-        private static  Vector3 down = new Vector3(0f, -1f, 0f);

-        private static  Vector3 right = new Vector3(1f, 0f, 0f);

-        private static Vector3 left = new Vector3(-1f, 0f, 0f);

-        private static Vector3 forward = new Vector3(0f, 0f, -1f);

-        private static Vector3 backward = new Vector3(0f, 0f, 1f);

-

-        #endregion Private Fields

-

-

-        #region Public Fields

-

-        public float X;

-        public float Y;

-        public float Z;

-

-        #endregion Public Fields

-

-

-        #region Properties

-

-        public static Vector3 Zero

-        {

-            get { return zero; }

-        }

-

-        public static Vector3 One

-        {

-            get { return one; }

-        }

-

-        public static Vector3 UnitX

-        {

-            get { return unitX; }

-        }

-

-        public static Vector3 UnitY

-        {

-            get { return unitY; }

-        }

-

-        public static Vector3 UnitZ

-        {

-            get { return unitZ; }

-        }

-

-        public static Vector3 Up

-        {

-            get { return up; }

-        }

-

-        public static Vector3 Down

-        {

-            get { return down; }

-        }

-

-        public static Vector3 Right

-        {

-            get { return right; }

-        }

-

-        public static Vector3 Left

-        {

-            get { return left; }

-        }

-

-        public static Vector3 Forward

-        {

-            get { return forward; }

-        }

-

-        public static Vector3 Backward

-        {

-            get { return backward; }

-        }

-

-        #endregion Properties

-

-

-        #region Constructors

-

-        public Vector3(float x, float y, float z)

-        {

-            this.X = x;

-            this.Y = y;

-            this.Z = z;

-        }

-

-

-        public Vector3(float value)

-        {

-            this.X = value;

-            this.Y = value;

-            this.Z = value;

-        }

-

-

-        /*public Vector3(Vector2 value, float z)

-        {

-            this.X = value.X;

-            this.Y = value.Y;

-            this.Z = z;

-        }*/

-

-

-        #endregion Constructors

-

-

-        #region Public Methods

-

-        public static Vector3 Add(Vector3 value1, Vector3 value2)

-        {

-            value1.X += value2.X;

-            value1.Y += value2.Y;

-            value1.Z += value2.Z;

-            return value1;

-        }

-

-        public static void Add(ref Vector3 value1, ref Vector3 value2, out Vector3 result)

-        {

-            result.X = value1.X + value2.X;

-            result.Y = value1.Y + value2.Y;

-            result.Z = value1.Z + value2.Z;

-        }

-

-        /*public static Vector3 Barycentric(Vector3 value1, Vector3 value2, Vector3 value3, float amount1, float amount2)

-        {

-            return new Vector3(

-                MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2),

-                MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2),

-                MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2));

-        }*/

-

-        /*public static void Barycentric(ref Vector3 value1, ref Vector3 value2, ref Vector3 value3, float amount1, float amount2, out Vector3 result)

-        {

-            result = new Vector3(

-                MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2),

-                MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2),

-                MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2));

-        }*/

-

-        /*public static Vector3 CatmullRom(Vector3 value1, Vector3 value2, Vector3 value3, Vector3 value4, float amount)

-        {

-            return new Vector3(

-                MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),

-                MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount),

-                MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount));

-        }*/

-

-        /*public static void CatmullRom(ref Vector3 value1, ref Vector3 value2, ref Vector3 value3, ref Vector3 value4, float amount, out Vector3 result)

-        {

-            result = new Vector3(

-                MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),

-                MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount),

-                MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount));

-        }*/

-

-        /*public static Vector3 Clamp(Vector3 value1, Vector3 min, Vector3 max)

-        {

-            return new Vector3(

-                MathHelper.Clamp(value1.X, min.X, max.X),

-                MathHelper.Clamp(value1.Y, min.Y, max.Y),

-                MathHelper.Clamp(value1.Z, min.Z, max.Z));

-        }*/

-

-        /*public static void Clamp(ref Vector3 value1, ref Vector3 min, ref Vector3 max, out Vector3 result)

-        {

-            result = new Vector3(

-                MathHelper.Clamp(value1.X, min.X, max.X),

-                MathHelper.Clamp(value1.Y, min.Y, max.Y),

-                MathHelper.Clamp(value1.Z, min.Z, max.Z));

-        }*/

-

-        public static Vector3 Cross(Vector3 vector1, Vector3 vector2)

-        {

-            Cross(ref vector1, ref vector2, out vector1);

-            return vector1;

-        }

-

-        public static void Cross(ref Vector3 vector1, ref Vector3 vector2, out Vector3 result)

-        {

-            result = new Vector3(vector1.Y * vector2.Z - vector2.Y * vector1.Z,

-                                 -(vector1.X * vector2.Z - vector2.X * vector1.Z),

-                                 vector1.X * vector2.Y - vector2.X * vector1.Y);

-        }

-

-        public static float Distance(Vector3 vector1, Vector3 vector2)

-        {

-            float result;

-            DistanceSquared(ref vector1, ref vector2, out result);

-            return (float)Math.Sqrt(result);

-        }

-

-        public static void Distance(ref Vector3 value1, ref Vector3 value2, out float result)

-        {

-            DistanceSquared(ref value1, ref value2, out result);

-            result = (float)Math.Sqrt(result);

-        }

-

-        public static float DistanceSquared(Vector3 value1, Vector3 value2)

-        {

-            float result;

-            DistanceSquared(ref value1, ref value2, out result);

-            return result;

-        }

-

-        public static void DistanceSquared(ref Vector3 value1, ref Vector3 value2, out float result)

-        {

-            result = (value1.X - value2.X) * (value1.X - value2.X) +

-                     (value1.Y - value2.Y) * (value1.Y - value2.Y) +

-                     (value1.Z - value2.Z) * (value1.Z - value2.Z);

-        }

-

-        public static Vector3 Divide(Vector3 value1, Vector3 value2)

-        {

-            value1.X /= value2.X;

-            value1.Y /= value2.Y;

-            value1.Z /= value2.Z;

-            return value1;

-        }

-

-        public static Vector3 Divide(Vector3 value1, float value2)

-        {

-            float factor = 1 / value2;

-            value1.X *= factor;

-            value1.Y *= factor;

-            value1.Z *= factor;

-            return value1;

-        }

-

-        public static void Divide(ref Vector3 value1, float divisor, out Vector3 result)

-        {

-            float factor = 1 / divisor;

-            result.X = value1.X * factor;

-            result.Y = value1.Y * factor;

-            result.Z = value1.Z * factor;

-        }

-

-        public static void Divide(ref Vector3 value1, ref Vector3 value2, out Vector3 result)

-        {

-            result.X = value1.X / value2.X;

-            result.Y = value1.Y / value2.Y;

-            result.Z = value1.Z / value2.Z;

-        }

-

-        public static float Dot(Vector3 vector1, Vector3 vector2)

-        {

-            return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z;

-        }

-

-        public static void Dot(ref Vector3 vector1, ref Vector3 vector2, out float result)

-        {

-            result = vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z;

-        }

-

-        public override bool Equals(object obj)

-        {

-            return (obj is Vector3) ? this == (Vector3)obj : false;

-        }

-

-        public bool Equals(Vector3 other)

-        {

-            return this == other;

-        }

-

-        public override int GetHashCode()

-        {

-            return (int)(this.X + this.Y + this.Z);

-        }

-

-        /*public static Vector3 Hermite(Vector3 value1, Vector3 tangent1, Vector3 value2, Vector3 tangent2, float amount)

-        {

-            Vector3 result = new Vector3();

-            Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result);

-            return result;

-        }*/

-

-        /*public static void Hermite(ref Vector3 value1, ref Vector3 tangent1, ref Vector3 value2, ref Vector3 tangent2, float amount, out Vector3 result)

-        {

-            result.X = MathHelper.Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount);

-            result.Y = MathHelper.Hermite(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount);

-            result.Z = MathHelper.Hermite(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount);

-        }*/

-

-        public float Length()

-        {

-            float result;

-            DistanceSquared(ref this, ref zero, out result);

-            return (float)Math.Sqrt(result);

-        }

-

-        public float LengthSquared()

-        {

-            float result;

-            DistanceSquared(ref this, ref zero, out result);

-            return result;

-        }

-

-        /*public static Vector3 Lerp(Vector3 value1, Vector3 value2, float amount)

-        {

-            return new Vector3(

-                MathHelper.Lerp(value1.X, value2.X, amount),

-                MathHelper.Lerp(value1.Y, value2.Y, amount),

-                MathHelper.Lerp(value1.Z, value2.Z, amount));

-        }*/

-

-        /*public static void Lerp(ref Vector3 value1, ref Vector3 value2, float amount, out Vector3 result)

-        {

-            result = new Vector3(

-                MathHelper.Lerp(value1.X, value2.X, amount),

-                MathHelper.Lerp(value1.Y, value2.Y, amount),

-                MathHelper.Lerp(value1.Z, value2.Z, amount));

-        }*/

-                

-        /*public static Vector3 Max(Vector3 value1, Vector3 value2)

-        {

-            return new Vector3(

-                MathHelper.Max(value1.X, value2.X),

-                MathHelper.Max(value1.Y, value2.Y),

-                MathHelper.Max(value1.Z, value2.Z));

-        }*/

-

-        /*public static void Max(ref Vector3 value1, ref Vector3 value2, out Vector3 result)

-        {

-            result = new Vector3(

-                MathHelper.Max(value1.X, value2.X),

-                MathHelper.Max(value1.Y, value2.Y),

-                MathHelper.Max(value1.Z, value2.Z));

-        }*/

-        

-        /*public static Vector3 Min(Vector3 value1, Vector3 value2)

-        {

-            return new Vector3(

-                MathHelper.Min(value1.X, value2.X),

-                MathHelper.Min(value1.Y, value2.Y),

-                MathHelper.Min(value1.Z, value2.Z));

-        }*/

-

-        /*public static void Min(ref Vector3 value1, ref Vector3 value2, out Vector3 result)

-        {

-            result = new Vector3(

-                MathHelper.Min(value1.X, value2.X),

-                MathHelper.Min(value1.Y, value2.Y),

-                MathHelper.Min(value1.Z, value2.Z));

-        }*/

-

-        public static Vector3 Multiply(Vector3 value1, Vector3 value2)

-        {

-            value1.X *= value2.X;

-            value1.Y *= value2.Y;

-            value1.Z *= value2.Z;

-            return value1;

-        }

-

-        public static Vector3 Multiply(Vector3 value1, float scaleFactor)

-        {

-            value1.X *= scaleFactor;

-            value1.Y *= scaleFactor;

-            value1.Z *= scaleFactor;

-            return value1;

-        }

-

-        public static void Multiply(ref Vector3 value1, float scaleFactor, out Vector3 result)

-        {

-            result.X = value1.X * scaleFactor;

-            result.Y = value1.Y * scaleFactor;

-            result.Z = value1.Z * scaleFactor;

-        }

-

-        public static void Multiply(ref Vector3 value1, ref Vector3 value2, out Vector3 result)

-        {

-            result.X = value1.X * value2.X;

-            result.Y = value1.Y * value2.Y;

-            result.Z = value1.Z * value2.Z;

-        }

-

-        public static Vector3 Negate(Vector3 value)

-        {

-            value = new Vector3(-value.X, -value.Y, -value.Z);

-            return value;

-        }

-

-        public static void Negate(ref Vector3 value, out Vector3 result)

-        {

-            result = new Vector3(-value.X, -value.Y, -value.Z);

-        }

-

-        public void Normalize()

-        {

-            Normalize(ref this, out this);

-        }

-

-        public static Vector3 Normalize(Vector3 vector)

-        {

-            Normalize(ref vector, out vector);

-            return vector;

-        }

-

-        public static void Normalize(ref Vector3 value, out Vector3 result)

-        {

-            float factor;

-            Distance(ref value, ref zero, out factor);

-            factor = 1f / factor;

-            result.X = value.X * factor;

-            result.Y = value.Y * factor;

-            result.Z = value.Z * factor;

-        }

-

-        public static Vector3 Reflect(Vector3 vector, Vector3 normal)

-        {

-            throw new NotImplementedException();

-        }

-

-        public static void Reflect(ref Vector3 vector, ref Vector3 normal, out Vector3 result)

-        {

-            throw new NotImplementedException();

-        }

-

-        /*public static Vector3 SmoothStep(Vector3 value1, Vector3 value2, float amount)

-        {

-            return new Vector3(

-                MathHelper.SmoothStep(value1.X, value2.X, amount),

-                MathHelper.SmoothStep(value1.Y, value2.Y, amount),

-                MathHelper.SmoothStep(value1.Z, value2.Z, amount));

-        }*/

-

-        /*public static void SmoothStep(ref Vector3 value1, ref Vector3 value2, float amount, out Vector3 result)

-        {

-            result = new Vector3(

-                MathHelper.SmoothStep(value1.X, value2.X, amount),

-                MathHelper.SmoothStep(value1.Y, value2.Y, amount),

-                MathHelper.SmoothStep(value1.Z, value2.Z, amount));

-        }*/

-

-        public static Vector3 Subtract(Vector3 value1, Vector3 value2)

-        {

-            value1.X -= value2.X;

-            value1.Y -= value2.Y;

-            value1.Z -= value2.Z;

-            return value1;

-        }

-

-        public static void Subtract(ref Vector3 value1, ref Vector3 value2, out Vector3 result)

-        {

-            result.X = value1.X - value2.X;

-            result.Y = value1.Y - value2.Y;

-            result.Z = value1.Z - value2.Z;

-        }

-

-        public override string ToString()

-        {

-            StringBuilder sb = new StringBuilder(32);

-            sb.Append("{X:");

-            sb.Append(this.X);

-            sb.Append(" Y:");

-            sb.Append(this.Y);

-            sb.Append(" Z:");

-            sb.Append(this.Z);

-            sb.Append("}");

-            return sb.ToString();

-        }

-

-        public static Vector3 Transform(Vector3 position, Matrix matrix)

-        {

-            Transform(ref position, ref matrix, out position);

-            return position;

-        }

-

-        public static void Transform(ref Vector3 position, ref Matrix matrix, out Vector3 result)

-        {

-            result = new Vector3((position.X * matrix.M11) + (position.Y * matrix.M21) + (position.Z * matrix.M31) + matrix.M41,

-                                 (position.X * matrix.M12) + (position.Y * matrix.M22) + (position.Z * matrix.M32) + matrix.M42,

-                                 (position.X * matrix.M13) + (position.Y * matrix.M23) + (position.Z * matrix.M33) + matrix.M43);

-        }

-

-        public static Vector3 TransformNormal(Vector3 normal, Matrix matrix)

-        {

-            TransformNormal(ref normal, ref matrix, out normal);

-            return normal;

-        }

-

-        public static void TransformNormal(ref Vector3 normal, ref Matrix matrix, out Vector3 result)

-        {

-            result = new Vector3((normal.X * matrix.M11) + (normal.Y * matrix.M21) + (normal.Z * matrix.M31),

-                                 (normal.X * matrix.M12) + (normal.Y * matrix.M22) + (normal.Z * matrix.M32),

-                                 (normal.X * matrix.M13) + (normal.Y * matrix.M23) + (normal.Z * matrix.M33));

-        }

-

-        #endregion Public methods

-

-

-        #region Operators

-

-        public static bool operator ==(Vector3 value1, Vector3 value2)

-        {

-            return value1.X == value2.X

-                && value1.Y == value2.Y

-                && value1.Z == value2.Z;

-        }

-

-        public static bool operator !=(Vector3 value1, Vector3 value2)

-        {

-            return !(value1 == value2);

-        }

-

-        public static Vector3 operator +(Vector3 value1, Vector3 value2)

-        {

-            value1.X += value2.X;

-            value1.Y += value2.Y;

-            value1.Z += value2.Z;

-            return value1;

-        }

-

-        public static Vector3 operator -(Vector3 value)

-        {

-            value = new Vector3(-value.X, -value.Y, -value.Z);

-            return value;

-        }

-

-        public static Vector3 operator -(Vector3 value1, Vector3 value2)

-        {

-            value1.X -= value2.X;

-            value1.Y -= value2.Y;

-            value1.Z -= value2.Z;

-            return value1;

-        }

-

-        public static Vector3 operator *(Vector3 value1, Vector3 value2)

-        {

-            value1.X *= value2.X;

-            value1.Y *= value2.Y;

-            value1.Z *= value2.Z;

-            return value1;

-        }

-

-        public static Vector3 operator *(Vector3 value, float scaleFactor)

-        {

-            value.X *= scaleFactor;

-            value.Y *= scaleFactor;

-            value.Z *= scaleFactor;

-            return value;

-        }

-

-        public static Vector3 operator *(float scaleFactor, Vector3 value)

-        {

-            value.X *= scaleFactor;

-            value.Y *= scaleFactor;

-            value.Z *= scaleFactor;

-            return value;

-        }

-

-        public static Vector3 operator /(Vector3 value1, Vector3 value2)

-        {

-            value1.X /= value2.X;

-            value1.Y /= value2.Y;

-            value1.Z /= value2.Z;

-            return value1;

-        }

-

-        public static Vector3 operator /(Vector3 value, float divider)

-        {

-            float factor = 1 / divider;

-            value.X *= factor;

-            value.Y *= factor;

-            value.Z *= factor;

-            return value;

-        }

-

-        #endregion

-    }

-}

+#region License
+/*
+MIT License
+Copyright © 2006 The Mono.Xna Team
+
+All rights reserved.
+
+Authors:
+ * Alan McGovern
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+#endregion License
+
+using System;
+using System.ComponentModel;
+using System.Text;
+using System.Runtime.InteropServices;
+
+namespace MonoXnaCompactMaths
+{
+    [Serializable]
+    [StructLayout(LayoutKind.Sequential)]
+    //[TypeConverter(typeof(Vector3Converter))]
+    public struct Vector3 : IEquatable<Vector3>
+    {
+        #region Private Fields
+
+        private static  Vector3 zero = new Vector3(0f, 0f, 0f);
+        private static  Vector3 one = new Vector3(1f, 1f, 1f);
+        private static  Vector3 unitX = new Vector3(1f, 0f, 0f);
+        private static  Vector3 unitY = new Vector3(0f, 1f, 0f);
+        private static  Vector3 unitZ = new Vector3(0f, 0f, 1f);
+        private static  Vector3 up = new Vector3(0f, 1f, 0f);
+        private static  Vector3 down = new Vector3(0f, -1f, 0f);
+        private static  Vector3 right = new Vector3(1f, 0f, 0f);
+        private static Vector3 left = new Vector3(-1f, 0f, 0f);
+        private static Vector3 forward = new Vector3(0f, 0f, -1f);
+        private static Vector3 backward = new Vector3(0f, 0f, 1f);
+
+        #endregion Private Fields
+
+
+        #region Public Fields
+
+        public float X;
+        public float Y;
+        public float Z;
+
+        #endregion Public Fields
+
+
+        #region Properties
+
+        public static Vector3 Zero
+        {
+            get { return zero; }
+        }
+
+        public static Vector3 One
+        {
+            get { return one; }
+        }
+
+        public static Vector3 UnitX
+        {
+            get { return unitX; }
+        }
+
+        public static Vector3 UnitY
+        {
+            get { return unitY; }
+        }
+
+        public static Vector3 UnitZ
+        {
+            get { return unitZ; }
+        }
+
+        public static Vector3 Up
+        {
+            get { return up; }
+        }
+
+        public static Vector3 Down
+        {
+            get { return down; }
+        }
+
+        public static Vector3 Right
+        {
+            get { return right; }
+        }
+
+        public static Vector3 Left
+        {
+            get { return left; }
+        }
+
+        public static Vector3 Forward
+        {
+            get { return forward; }
+        }
+
+        public static Vector3 Backward
+        {
+            get { return backward; }
+        }
+
+        #endregion Properties
+
+
+        #region Constructors
+
+        public Vector3(float x, float y, float z)
+        {
+            this.X = x;
+            this.Y = y;
+            this.Z = z;
+        }
+
+
+        public Vector3(float value)
+        {
+            this.X = value;
+            this.Y = value;
+            this.Z = value;
+        }
+
+
+        /*public Vector3(Vector2 value, float z)
+        {
+            this.X = value.X;
+            this.Y = value.Y;
+            this.Z = z;
+        }*/
+
+
+        #endregion Constructors
+
+
+        #region Public Methods
+
+        public static Vector3 Add(Vector3 value1, Vector3 value2)
+        {
+            value1.X += value2.X;
+            value1.Y += value2.Y;
+            value1.Z += value2.Z;
+            return value1;
+        }
+
+        public static void Add(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
+        {
+            result.X = value1.X + value2.X;
+            result.Y = value1.Y + value2.Y;
+            result.Z = value1.Z + value2.Z;
+        }
+
+        /*public static Vector3 Barycentric(Vector3 value1, Vector3 value2, Vector3 value3, float amount1, float amount2)
+        {
+            return new Vector3(
+                MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2),
+                MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2),
+                MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2));
+        }*/
+
+        /*public static void Barycentric(ref Vector3 value1, ref Vector3 value2, ref Vector3 value3, float amount1, float amount2, out Vector3 result)
+        {
+            result = new Vector3(
+                MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2),
+                MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2),
+                MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2));
+        }*/
+
+        /*public static Vector3 CatmullRom(Vector3 value1, Vector3 value2, Vector3 value3, Vector3 value4, float amount)
+        {
+            return new Vector3(
+                MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),
+                MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount),
+                MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount));
+        }*/
+
+        /*public static void CatmullRom(ref Vector3 value1, ref Vector3 value2, ref Vector3 value3, ref Vector3 value4, float amount, out Vector3 result)
+        {
+            result = new Vector3(
+                MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),
+                MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount),
+                MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount));
+        }*/
+
+        /*public static Vector3 Clamp(Vector3 value1, Vector3 min, Vector3 max)
+        {
+            return new Vector3(
+                MathHelper.Clamp(value1.X, min.X, max.X),
+                MathHelper.Clamp(value1.Y, min.Y, max.Y),
+                MathHelper.Clamp(value1.Z, min.Z, max.Z));
+        }*/
+
+        /*public static void Clamp(ref Vector3 value1, ref Vector3 min, ref Vector3 max, out Vector3 result)
+        {
+            result = new Vector3(
+                MathHelper.Clamp(value1.X, min.X, max.X),
+                MathHelper.Clamp(value1.Y, min.Y, max.Y),
+                MathHelper.Clamp(value1.Z, min.Z, max.Z));
+        }*/
+
+        public static Vector3 Cross(Vector3 vector1, Vector3 vector2)
+        {
+            Cross(ref vector1, ref vector2, out vector1);
+            return vector1;
+        }
+
+        public static void Cross(ref Vector3 vector1, ref Vector3 vector2, out Vector3 result)
+        {
+            result = new Vector3(vector1.Y * vector2.Z - vector2.Y * vector1.Z,
+                                 -(vector1.X * vector2.Z - vector2.X * vector1.Z),
+                                 vector1.X * vector2.Y - vector2.X * vector1.Y);
+        }
+
+        public static float Distance(Vector3 vector1, Vector3 vector2)
+        {
+            float result;
+            DistanceSquared(ref vector1, ref vector2, out result);
+            return (float)Math.Sqrt(result);
+        }
+
+        public static void Distance(ref Vector3 value1, ref Vector3 value2, out float result)
+        {
+            DistanceSquared(ref value1, ref value2, out result);
+            result = (float)Math.Sqrt(result);
+        }
+
+        public static float DistanceSquared(Vector3 value1, Vector3 value2)
+        {
+            float result;
+            DistanceSquared(ref value1, ref value2, out result);
+            return result;
+        }
+
+        public static void DistanceSquared(ref Vector3 value1, ref Vector3 value2, out float result)
+        {
+            result = (value1.X - value2.X) * (value1.X - value2.X) +
+                     (value1.Y - value2.Y) * (value1.Y - value2.Y) +
+                     (value1.Z - value2.Z) * (value1.Z - value2.Z);
+        }
+
+        public static Vector3 Divide(Vector3 value1, Vector3 value2)
+        {
+            value1.X /= value2.X;
+            value1.Y /= value2.Y;
+            value1.Z /= value2.Z;
+            return value1;
+        }
+
+        public static Vector3 Divide(Vector3 value1, float value2)
+        {
+            float factor = 1 / value2;
+            value1.X *= factor;
+            value1.Y *= factor;
+            value1.Z *= factor;
+            return value1;
+        }
+
+        public static void Divide(ref Vector3 value1, float divisor, out Vector3 result)
+        {
+            float factor = 1 / divisor;
+            result.X = value1.X * factor;
+            result.Y = value1.Y * factor;
+            result.Z = value1.Z * factor;
+        }
+
+        public static void Divide(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
+        {
+            result.X = value1.X / value2.X;
+            result.Y = value1.Y / value2.Y;
+            result.Z = value1.Z / value2.Z;
+        }
+
+        public static float Dot(Vector3 vector1, Vector3 vector2)
+        {
+            return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z;
+        }
+
+        public static void Dot(ref Vector3 vector1, ref Vector3 vector2, out float result)
+        {
+            result = vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z;
+        }
+
+        public override bool Equals(object obj)
+        {
+            return (obj is Vector3) ? this == (Vector3)obj : false;
+        }
+
+        public bool Equals(Vector3 other)
+        {
+            return this == other;
+        }
+
+        public override int GetHashCode()
+        {
+            return (int)(this.X + this.Y + this.Z);
+        }
+
+        /*public static Vector3 Hermite(Vector3 value1, Vector3 tangent1, Vector3 value2, Vector3 tangent2, float amount)
+        {
+            Vector3 result = new Vector3();
+            Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result);
+            return result;
+        }*/
+
+        /*public static void Hermite(ref Vector3 value1, ref Vector3 tangent1, ref Vector3 value2, ref Vector3 tangent2, float amount, out Vector3 result)
+        {
+            result.X = MathHelper.Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount);
+            result.Y = MathHelper.Hermite(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount);
+            result.Z = MathHelper.Hermite(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount);
+        }*/
+
+        public float Length()
+        {
+            float result;
+            DistanceSquared(ref this, ref zero, out result);
+            return (float)Math.Sqrt(result);
+        }
+
+        public float LengthSquared()
+        {
+            float result;
+            DistanceSquared(ref this, ref zero, out result);
+            return result;
+        }
+
+        /*public static Vector3 Lerp(Vector3 value1, Vector3 value2, float amount)
+        {
+            return new Vector3(
+                MathHelper.Lerp(value1.X, value2.X, amount),
+                MathHelper.Lerp(value1.Y, value2.Y, amount),
+                MathHelper.Lerp(value1.Z, value2.Z, amount));
+        }*/
+
+        /*public static void Lerp(ref Vector3 value1, ref Vector3 value2, float amount, out Vector3 result)
+        {
+            result = new Vector3(
+                MathHelper.Lerp(value1.X, value2.X, amount),
+                MathHelper.Lerp(value1.Y, value2.Y, amount),
+                MathHelper.Lerp(value1.Z, value2.Z, amount));
+        }*/
+                
+        /*public static Vector3 Max(Vector3 value1, Vector3 value2)
+        {
+            return new Vector3(
+                MathHelper.Max(value1.X, value2.X),
+                MathHelper.Max(value1.Y, value2.Y),
+                MathHelper.Max(value1.Z, value2.Z));
+        }*/
+
+        /*public static void Max(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
+        {
+            result = new Vector3(
+                MathHelper.Max(value1.X, value2.X),
+                MathHelper.Max(value1.Y, value2.Y),
+                MathHelper.Max(value1.Z, value2.Z));
+        }*/
+        
+        /*public static Vector3 Min(Vector3 value1, Vector3 value2)
+        {
+            return new Vector3(
+                MathHelper.Min(value1.X, value2.X),
+                MathHelper.Min(value1.Y, value2.Y),
+                MathHelper.Min(value1.Z, value2.Z));
+        }*/
+
+        /*public static void Min(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
+        {
+            result = new Vector3(
+                MathHelper.Min(value1.X, value2.X),
+                MathHelper.Min(value1.Y, value2.Y),
+                MathHelper.Min(value1.Z, value2.Z));
+        }*/
+
+        public static Vector3 Multiply(Vector3 value1, Vector3 value2)
+        {
+            value1.X *= value2.X;
+            value1.Y *= value2.Y;
+            value1.Z *= value2.Z;
+            return value1;
+        }
+
+        public static Vector3 Multiply(Vector3 value1, float scaleFactor)
+        {
+            value1.X *= scaleFactor;
+            value1.Y *= scaleFactor;
+            value1.Z *= scaleFactor;
+            return value1;
+        }
+
+        public static void Multiply(ref Vector3 value1, float scaleFactor, out Vector3 result)
+        {
+            result.X = value1.X * scaleFactor;
+            result.Y = value1.Y * scaleFactor;
+            result.Z = value1.Z * scaleFactor;
+        }
+
+        public static void Multiply(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
+        {
+            result.X = value1.X * value2.X;
+            result.Y = value1.Y * value2.Y;
+            result.Z = value1.Z * value2.Z;
+        }
+
+        public static Vector3 Negate(Vector3 value)
+        {
+            value = new Vector3(-value.X, -value.Y, -value.Z);
+            return value;
+        }
+
+        public static void Negate(ref Vector3 value, out Vector3 result)
+        {
+            result = new Vector3(-value.X, -value.Y, -value.Z);
+        }
+
+        public void Normalize()
+        {
+            Normalize(ref this, out this);
+        }
+
+        public static Vector3 Normalize(Vector3 vector)
+        {
+            Normalize(ref vector, out vector);
+            return vector;
+        }
+
+        public static void Normalize(ref Vector3 value, out Vector3 result)
+        {
+            float factor;
+            Distance(ref value, ref zero, out factor);
+            factor = 1f / factor;
+            result.X = value.X * factor;
+            result.Y = value.Y * factor;
+            result.Z = value.Z * factor;
+        }
+
+        public static Vector3 Reflect(Vector3 vector, Vector3 normal)
+        {
+            throw new NotImplementedException();
+        }
+
+        public static void Reflect(ref Vector3 vector, ref Vector3 normal, out Vector3 result)
+        {
+            throw new NotImplementedException();
+        }
+
+        /*public static Vector3 SmoothStep(Vector3 value1, Vector3 value2, float amount)
+        {
+            return new Vector3(
+                MathHelper.SmoothStep(value1.X, value2.X, amount),
+                MathHelper.SmoothStep(value1.Y, value2.Y, amount),
+                MathHelper.SmoothStep(value1.Z, value2.Z, amount));
+        }*/
+
+        /*public static void SmoothStep(ref Vector3 value1, ref Vector3 value2, float amount, out Vector3 result)
+        {
+            result = new Vector3(
+                MathHelper.SmoothStep(value1.X, value2.X, amount),
+                MathHelper.SmoothStep(value1.Y, value2.Y, amount),
+                MathHelper.SmoothStep(value1.Z, value2.Z, amount));
+        }*/
+
+        public static Vector3 Subtract(Vector3 value1, Vector3 value2)
+        {
+            value1.X -= value2.X;
+            value1.Y -= value2.Y;
+            value1.Z -= value2.Z;
+            return value1;
+        }
+
+        public static void Subtract(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
+        {
+            result.X = value1.X - value2.X;
+            result.Y = value1.Y - value2.Y;
+            result.Z = value1.Z - value2.Z;
+        }
+
+        public override string ToString()
+        {
+            StringBuilder sb = new StringBuilder(32);
+            sb.Append("{X:");
+            sb.Append(this.X);
+            sb.Append(" Y:");
+            sb.Append(this.Y);
+            sb.Append(" Z:");
+            sb.Append(this.Z);
+            sb.Append("}");
+            return sb.ToString();
+        }
+
+        public static Vector3 Transform(Vector3 position, Matrix matrix)
+        {
+            Transform(ref position, ref matrix, out position);
+            return position;
+        }
+
+        public static void Transform(ref Vector3 position, ref Matrix matrix, out Vector3 result)
+        {
+            result = new Vector3((position.X * matrix.M11) + (position.Y * matrix.M21) + (position.Z * matrix.M31) + matrix.M41,
+                                 (position.X * matrix.M12) + (position.Y * matrix.M22) + (position.Z * matrix.M32) + matrix.M42,
+                                 (position.X * matrix.M13) + (position.Y * matrix.M23) + (position.Z * matrix.M33) + matrix.M43);
+        }
+
+        public static Vector3 TransformNormal(Vector3 normal, Matrix matrix)
+        {
+            TransformNormal(ref normal, ref matrix, out normal);
+            return normal;
+        }
+
+        public static void TransformNormal(ref Vector3 normal, ref Matrix matrix, out Vector3 result)
+        {
+            result = new Vector3((normal.X * matrix.M11) + (normal.Y * matrix.M21) + (normal.Z * matrix.M31),
+                                 (normal.X * matrix.M12) + (normal.Y * matrix.M22) + (normal.Z * matrix.M32),
+                                 (normal.X * matrix.M13) + (normal.Y * matrix.M23) + (normal.Z * matrix.M33));
+        }
+
+        #endregion Public methods
+
+
+        #region Operators
+
+        public static bool operator ==(Vector3 value1, Vector3 value2)
+        {
+            return value1.X == value2.X
+                && value1.Y == value2.Y
+                && value1.Z == value2.Z;
+        }
+
+        public static bool operator !=(Vector3 value1, Vector3 value2)
+        {
+            return !(value1 == value2);
+        }
+
+        public static Vector3 operator +(Vector3 value1, Vector3 value2)
+        {
+            value1.X += value2.X;
+            value1.Y += value2.Y;
+            value1.Z += value2.Z;
+            return value1;
+        }
+
+        public static Vector3 operator -(Vector3 value)
+        {
+            value = new Vector3(-value.X, -value.Y, -value.Z);
+            return value;
+        }
+
+        public static Vector3 operator -(Vector3 value1, Vector3 value2)
+        {
+            value1.X -= value2.X;
+            value1.Y -= value2.Y;
+            value1.Z -= value2.Z;
+            return value1;
+        }
+
+        public static Vector3 operator *(Vector3 value1, Vector3 value2)
+        {
+            value1.X *= value2.X;
+            value1.Y *= value2.Y;
+            value1.Z *= value2.Z;
+            return value1;
+        }
+
+        public static Vector3 operator *(Vector3 value, float scaleFactor)
+        {
+            value.X *= scaleFactor;
+            value.Y *= scaleFactor;
+            value.Z *= scaleFactor;
+            return value;
+        }
+
+        public static Vector3 operator *(float scaleFactor, Vector3 value)
+        {
+            value.X *= scaleFactor;
+            value.Y *= scaleFactor;
+            value.Z *= scaleFactor;
+            return value;
+        }
+
+        public static Vector3 operator /(Vector3 value1, Vector3 value2)
+        {
+            value1.X /= value2.X;
+            value1.Y /= value2.Y;
+            value1.Z /= value2.Z;
+            return value1;
+        }
+
+        public static Vector3 operator /(Vector3 value, float divider)
+        {
+            float factor = 1 / divider;
+            value.X *= factor;
+            value.Y *= factor;
+            value.Z *= factor;
+            return value;
+        }
+
+        #endregion
+    }
+}
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector4.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector4.cs
index c75b7c8..abb30a8 100644
--- a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector4.cs
+++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector4.cs
@@ -1,630 +1,630 @@
-#region License

-/*

-MIT License

-Copyright © 2006 The Mono.Xna Team

-

-All rights reserved.

-

-Permission is hereby granted, free of charge, to any person obtaining a copy

-of this software and associated documentation files (the "Software"), to deal

-in the Software without restriction, including without limitation the rights

-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell

-copies of the Software, and to permit persons to whom the Software is

-furnished to do so, subject to the following conditions:

-

-The above copyright notice and this permission notice shall be included in all

-copies or substantial portions of the Software.

-

-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR

-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,

-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE

-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER

-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,

-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE

-SOFTWARE.

-*/

-#endregion License

-

-using System;

-using System.ComponentModel;

-using System.Text;

-using System.Runtime.InteropServices;

-

-namespace MonoXnaCompactMaths

-{

-    [Serializable]

-    [StructLayout(LayoutKind.Sequential)]

-    //[TypeConverter(typeof(Vector4Converter))]

-    public struct Vector4 : IEquatable<Vector4>

-    {

-        #region Private Fields

-

-        private static Vector4 zeroVector = new Vector4();

-        private static Vector4 unitVector = new Vector4(1f, 1f, 1f, 1f);

-        private static Vector4 unitXVector = new Vector4(1f, 0f, 0f, 0f);

-        private static Vector4 unitYVector = new Vector4(0f, 1f, 0f, 0f);

-        private static Vector4 unitZVector = new Vector4(0f, 0f, 1f, 0f);

-        private static Vector4 unitWVector = new Vector4(0f, 0f, 0f, 1f);

-

-        #endregion Private Fields

-

-

-        #region Public Fields

-

-        public float X;

-        public float Y;

-        public float Z;

-        public float W;

-

-        #endregion Public Fields

-

-

-        #region Properties

-

-        public static Vector4 Zero

-        {

-            get { return zeroVector; }

-        }

-

-        public static Vector4 One

-        {

-            get { return unitVector; }

-        }

-

-        public static Vector4 UnitX

-        {

-            get { return unitXVector; }

-        }

-

-        public static Vector4 UnitY

-        {

-            get { return unitYVector; }

-        }

-

-        public static Vector4 UnitZ

-        {

-            get { return unitZVector; }

-        }

-

-        public static Vector4 UnitW

-        {

-            get { return unitWVector; }

-        }

-

-        #endregion Properties

-

-

-        #region Constructors

-

-        public Vector4(float x, float y, float z, float w)

-        {

-            this.X = x;

-            this.Y = y;

-            this.Z = z;

-            this.W = w;

-        }

-

-        /*public Vector4(Vector2 value, float z, float w)

-        {

-            this.X = value.X;

-            this.Y = value.Y;

-            this.Z = z;

-            this.W = w;

-        }*/

-

-        public Vector4(Vector3 value, float w)

-        {

-            this.X = value.X;

-            this.Y = value.Y;

-            this.Z = value.Z;

-            this.W = w;

-        }

-

-        public Vector4(float value)

-        {

-            this.X = value;

-            this.Y = value;

-            this.Z = value;

-            this.W = value;

-        }

-

-        #endregion

-

-

-        #region Public Methods

-

-        public static Vector4 Add(Vector4 value1, Vector4 value2)

-        {

-            value1.W += value2.W;

-            value1.X += value2.X;

-            value1.Y += value2.Y;

-            value1.Z += value2.Z;

-            return value1;

-        }

-

-        public static void Add(ref Vector4 value1, ref Vector4 value2, out Vector4 result)

-        {

-            result.W = value1.W + value2.W;

-            result.X = value1.X + value2.X;

-            result.Y = value1.Y + value2.Y;

-            result.Z = value1.Z + value2.Z;

-        }

-

-        /*public static Vector4 Barycentric(Vector4 value1, Vector4 value2, Vector4 value3, float amount1, float amount2)

-        {

-            return new Vector4(

-                MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2),

-                MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2),

-                MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2),

-                MathHelper.Barycentric(value1.W, value2.W, value3.W, amount1, amount2));

-        }*/

-

-        /*public static void Barycentric(ref Vector4 value1, ref Vector4 value2, ref Vector4 value3, float amount1, float amount2, out Vector4 result)

-        {

-            result = new Vector4(

-                MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2),

-                MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2),

-                MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2),

-                MathHelper.Barycentric(value1.W, value2.W, value3.W, amount1, amount2));

-        }*/

-

-        /*public static Vector4 CatmullRom(Vector4 value1, Vector4 value2, Vector4 value3, Vector4 value4, float amount)

-        {

-            return new Vector4(

-                MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),

-                MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount),

-                MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount),

-                MathHelper.CatmullRom(value1.W, value2.W, value3.W, value4.W, amount));

-        }*/

-

-        /*public static void CatmullRom(ref Vector4 value1, ref Vector4 value2, ref Vector4 value3, ref Vector4 value4, float amount, out Vector4 result)

-        {

-            result = new Vector4(

-                MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),

-                MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount),

-                MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount),

-                MathHelper.CatmullRom(value1.W, value2.W, value3.W, value4.W, amount));

-        }*/

-

-        /*public static Vector4 Clamp(Vector4 value1, Vector4 min, Vector4 max)

-        {

-            return new Vector4(

-                MathHelper.Clamp(value1.X, min.X, max.X),

-                MathHelper.Clamp(value1.Y, min.Y, max.Y),

-                MathHelper.Clamp(value1.Z, min.Z, max.Z),

-                MathHelper.Clamp(value1.W, min.W, max.W));

-        }*/

-

-        /*public static void Clamp(ref Vector4 value1, ref Vector4 min, ref Vector4 max, out Vector4 result)

-        {

-            result = new Vector4(

-                MathHelper.Clamp(value1.X, min.X, max.X),

-                MathHelper.Clamp(value1.Y, min.Y, max.Y),

-                MathHelper.Clamp(value1.Z, min.Z, max.Z),

-                MathHelper.Clamp(value1.W, min.W, max.W));

-        }*/

-

-        public static float Distance(Vector4 value1, Vector4 value2)

-        {

-            return (float)Math.Sqrt(DistanceSquared(value1, value2));

-        }

-

-        public static void Distance(ref Vector4 value1, ref Vector4 value2, out float result)

-        {

-            result = (float)Math.Sqrt(DistanceSquared(value1, value2));

-        }

-

-        public static float DistanceSquared(Vector4 value1, Vector4 value2)

-        {

-            float result;

-            DistanceSquared(ref value1, ref value2, out result);

-            return result;

-        }

-

-        public static void DistanceSquared(ref Vector4 value1, ref Vector4 value2, out float result)

-        {

-            result = (value1.W - value2.W) * (value1.W - value2.W) +

-                     (value1.X - value2.X) * (value1.X - value2.X) +

-                     (value1.Y - value2.Y) * (value1.Y - value2.Y) +

-                     (value1.Z - value2.Z) * (value1.Z - value2.Z);

-        }

-

-        public static Vector4 Divide(Vector4 value1, Vector4 value2)

-        {

-            value1.W /= value2.W;

-            value1.X /= value2.X;

-            value1.Y /= value2.Y;

-            value1.Z /= value2.Z;

-            return value1;

-        }

-

-        public static Vector4 Divide(Vector4 value1, float divider)

-        {

-            float factor = 1f / divider;

-            value1.W *= factor;

-            value1.X *= factor;

-            value1.Y *= factor;

-            value1.Z *= factor;

-            return value1;

-        }

-

-        public static void Divide(ref Vector4 value1, float divider, out Vector4 result)

-        {

-            float factor = 1f / divider;

-            result.W = value1.W * factor;

-            result.X = value1.X * factor;

-            result.Y = value1.Y * factor;

-            result.Z = value1.Z * factor;

-        }

-

-        public static void Divide(ref Vector4 value1, ref Vector4 value2, out Vector4 result)

-        {

-            result.W = value1.W / value2.W;

-            result.X = value1.X / value2.X;

-            result.Y = value1.Y / value2.Y;

-            result.Z = value1.Z / value2.Z;

-        }

-

-        public static float Dot(Vector4 vector1, Vector4 vector2)

-        {

-            return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z + vector1.W * vector2.W;

-        }

-

-        public static void Dot(ref Vector4 vector1, ref Vector4 vector2, out float result)

-        {

-            result = vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z + vector1.W * vector2.W;

-        }

-

-        public override bool Equals(object obj)

-        {

-            return (obj is Vector4) ? this == (Vector4)obj : false;

-        }

-

-        public bool Equals(Vector4 other)

-        {

-            return this.W == other.W

-                && this.X == other.X

-                && this.Y == other.Y

-                && this.Z == other.Z;

-        }

-

-        public override int GetHashCode()

-        {

-            return (int)(this.W + this.X + this.Y + this.Y);

-        }

-

-        /*public static Vector4 Hermite(Vector4 value1, Vector4 tangent1, Vector4 value2, Vector4 tangent2, float amount)

-        {

-            Vector4 result = new Vector4();

-            Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result);

-            return result;

-        }*/

-

-        /*public static void Hermite(ref Vector4 value1, ref Vector4 tangent1, ref Vector4 value2, ref Vector4 tangent2, float amount, out Vector4 result)

-        {

-            result.W = MathHelper.Hermite(value1.W, tangent1.W, value2.W, tangent2.W, amount);

-            result.X = MathHelper.Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount);

-            result.Y = MathHelper.Hermite(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount);

-            result.Z = MathHelper.Hermite(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount);

-        }*/

-

-        public float Length()

-        {

-            float result;

-            DistanceSquared(ref this, ref zeroVector, out result);

-            return (float)Math.Sqrt(result);

-        }

-

-        public float LengthSquared()

-        {

-            float result;

-            DistanceSquared(ref this, ref zeroVector, out result);

-            return result;

-        }

-

-        /*public static Vector4 Lerp(Vector4 value1, Vector4 value2, float amount)

-        {

-            return new Vector4(

-                MathHelper.Lerp(value1.X, value2.X, amount),

-                MathHelper.Lerp(value1.Y, value2.Y, amount),

-                MathHelper.Lerp(value1.Z, value2.Z, amount),

-                MathHelper.Lerp(value1.W, value2.W, amount));

-        }*/

-

-        /*public static void Lerp(ref Vector4 value1, ref Vector4 value2, float amount, out Vector4 result)

-        {

-            result = new Vector4(

-                MathHelper.Lerp(value1.X, value2.X, amount),

-                MathHelper.Lerp(value1.Y, value2.Y, amount),

-                MathHelper.Lerp(value1.Z, value2.Z, amount),

-                MathHelper.Lerp(value1.W, value2.W, amount));

-        }*/

-

-        /*public static Vector4 Max(Vector4 value1, Vector4 value2)

-        {

-            return new Vector4(

-               MathHelper.Max(value1.X, value2.X),

-               MathHelper.Max(value1.Y, value2.Y),

-               MathHelper.Max(value1.Z, value2.Z),

-               MathHelper.Max(value1.W, value2.W));

-        }*/

-

-        /*public static void Max(ref Vector4 value1, ref Vector4 value2, out Vector4 result)

-        {

-            result = new Vector4(

-               MathHelper.Max(value1.X, value2.X),

-               MathHelper.Max(value1.Y, value2.Y),

-               MathHelper.Max(value1.Z, value2.Z),

-               MathHelper.Max(value1.W, value2.W));

-        }*/

-

-        /*public static Vector4 Min(Vector4 value1, Vector4 value2)

-        {

-            return new Vector4(

-               MathHelper.Min(value1.X, value2.X),

-               MathHelper.Min(value1.Y, value2.Y),

-               MathHelper.Min(value1.Z, value2.Z),

-               MathHelper.Min(value1.W, value2.W));

-        }*/

-

-        /*public static void Min(ref Vector4 value1, ref Vector4 value2, out Vector4 result)

-        {

-            result = new Vector4(

-               MathHelper.Min(value1.X, value2.X),

-               MathHelper.Min(value1.Y, value2.Y),

-               MathHelper.Min(value1.Z, value2.Z),

-               MathHelper.Min(value1.W, value2.W));

-        }*/

-

-        public static Vector4 Multiply(Vector4 value1, Vector4 value2)

-        {

-            value1.W *= value2.W;

-            value1.X *= value2.X;

-            value1.Y *= value2.Y;

-            value1.Z *= value2.Z;

-            return value1;

-        }

-

-        public static Vector4 Multiply(Vector4 value1, float scaleFactor)

-        {

-            value1.W *= scaleFactor;

-            value1.X *= scaleFactor;

-            value1.Y *= scaleFactor;

-            value1.Z *= scaleFactor;

-            return value1;

-        }

-

-        public static void Multiply(ref Vector4 value1, float scaleFactor, out Vector4 result)

-        {

-            result.W = value1.W * scaleFactor;

-            result.X = value1.X * scaleFactor;

-            result.Y = value1.Y * scaleFactor;

-            result.Z = value1.Z * scaleFactor;

-        }

-

-        public static void Multiply(ref Vector4 value1, ref Vector4 value2, out Vector4 result)

-        {

-            result.W = value1.W * value2.W;

-            result.X = value1.X * value2.X;

-            result.Y = value1.Y * value2.Y;

-            result.Z = value1.Z * value2.Z;

-        }

-

-        public static Vector4 Negate(Vector4 value)

-        {

-            value = new Vector4(-value.X, -value.Y, -value.Z, -value.W);

-            return value;

-        }

-

-        public static void Negate(ref Vector4 value, out Vector4 result)

-        {

-            result = new Vector4(-value.X, -value.Y, -value.Z,-value.W);

-        }

-

-        public void Normalize()

-        {

-            Normalize(ref this, out this);

-        }

-

-        public static Vector4 Normalize(Vector4 vector)

-        {

-            Normalize(ref vector, out vector);

-            return vector;

-        }

-

-        public static void Normalize(ref Vector4 vector, out Vector4 result)

-        {

-            float factor;

-            DistanceSquared(ref vector, ref zeroVector, out factor);

-            factor = 1f / (float)Math.Sqrt(factor);

-

-            result.W = vector.W * factor;

-            result.X = vector.X * factor;

-            result.Y = vector.Y * factor;

-            result.Z = vector.Z * factor;

-        }

-

-        /*public static Vector4 SmoothStep(Vector4 value1, Vector4 value2, float amount)

-        {

-            return new Vector4(

-                MathHelper.SmoothStep(value1.X, value2.X, amount),

-                MathHelper.SmoothStep(value1.Y, value2.Y, amount),

-                MathHelper.SmoothStep(value1.Z, value2.Z, amount),

-                MathHelper.SmoothStep(value1.W, value2.W, amount));

-        }*/

-

-        /*public static void SmoothStep(ref Vector4 value1, ref Vector4 value2, float amount, out Vector4 result)

-        {

-            result = new Vector4(

-                MathHelper.SmoothStep(value1.X, value2.X, amount),

-                MathHelper.SmoothStep(value1.Y, value2.Y, amount),

-                MathHelper.SmoothStep(value1.Z, value2.Z, amount),

-                MathHelper.SmoothStep(value1.W, value2.W, amount));

-        }*/

-

-        public static Vector4 Subtract(Vector4 value1, Vector4 value2)

-        {

-            value1.W -= value2.W;

-            value1.X -= value2.X;

-            value1.Y -= value2.Y;

-            value1.Z -= value2.Z;

-            return value1;

-        }

-

-        public static void Subtract(ref Vector4 value1, ref Vector4 value2, out Vector4 result)

-        {

-            result.W = value1.W - value2.W;

-            result.X = value1.X - value2.X;

-            result.Y = value1.Y - value2.Y;

-            result.Z = value1.Z - value2.Z;

-        }

-

-        /*public static Vector4 Transform(Vector2 position, Matrix matrix)

-        {

-            Vector4 result;

-            Transform(ref position, ref matrix, out result);

-            return result;

-        }*/

-

-        public static Vector4 Transform(Vector3 position, Matrix matrix)

-        {

-            Vector4 result;

-            Transform(ref position, ref matrix, out result);

-            return result;

-        }

-

-        public static Vector4 Transform(Vector4 vector, Matrix matrix)

-        {

-            Transform(ref vector, ref matrix, out vector);

-            return vector;

-        }

-

-        /*public static void Transform(ref Vector2 position, ref Matrix matrix, out Vector4 result)

-        {

-            result = new Vector4((position.X * matrix.M11) + (position.Y * matrix.M21) + matrix.M41,

-                                 (position.X * matrix.M12) + (position.Y * matrix.M22) + matrix.M42,

-                                 (position.X * matrix.M13) + (position.Y * matrix.M23) + matrix.M43,

-                                 (position.X * matrix.M14) + (position.Y * matrix.M24) + matrix.M44);

-        }*/

-

-        public static void Transform(ref Vector3 position, ref Matrix matrix, out Vector4 result)

-        {

-            result = new Vector4((position.X * matrix.M11) + (position.Y * matrix.M21) + (position.Z * matrix.M31) + matrix.M41,

-                                 (position.X * matrix.M12) + (position.Y * matrix.M22) + (position.Z * matrix.M32) + matrix.M42,

-                                 (position.X * matrix.M13) + (position.Y * matrix.M23) + (position.Z * matrix.M33) + matrix.M43,

-                                 (position.X * matrix.M14) + (position.Y * matrix.M24) + (position.Z * matrix.M34) + matrix.M44);

-        }

-

-        public static void Transform(ref Vector4 vector, ref Matrix matrix, out Vector4 result)

-        {

-            result = new Vector4((vector.X * matrix.M11) + (vector.Y * matrix.M21) + (vector.Z * matrix.M31) + (vector.W * matrix.M41),

-                                 (vector.X * matrix.M12) + (vector.Y * matrix.M22) + (vector.Z * matrix.M32) + (vector.W * matrix.M42),

-                                 (vector.X * matrix.M13) + (vector.Y * matrix.M23) + (vector.Z * matrix.M33) + (vector.W * matrix.M43),

-                                 (vector.X * matrix.M14) + (vector.Y * matrix.M24) + (vector.Z * matrix.M34) + (vector.W * matrix.M44));

-        }

-

-        public override string ToString()

-        {

-            StringBuilder sb = new StringBuilder(32);

-            sb.Append("{X:");

-            sb.Append(this.X);

-            sb.Append(" Y:");

-            sb.Append(this.Y);

-            sb.Append(" Z:");

-            sb.Append(this.Z);

-            sb.Append(" W:");

-            sb.Append(this.W);

-            sb.Append("}");

-            return sb.ToString();

-        }

-

-        #endregion Public Methods

-

-

-        #region Operators

-

-        public static Vector4 operator -(Vector4 value)

-        {

-            return new Vector4(-value.X, -value.Y, -value.Z, -value.W);

-        }

-

-        public static bool operator ==(Vector4 value1, Vector4 value2)

-        {

-            return value1.W == value2.W

-                && value1.X == value2.X

-                && value1.Y == value2.Y

-                && value1.Z == value2.Z;

-        }

-

-        public static bool operator !=(Vector4 value1, Vector4 value2)

-        {

-            return !(value1 == value2);

-        }

-

-        public static Vector4 operator +(Vector4 value1, Vector4 value2)

-        {

-            value1.W += value2.W;

-            value1.X += value2.X;

-            value1.Y += value2.Y;

-            value1.Z += value2.Z;

-            return value1;

-        }

-

-        public static Vector4 operator -(Vector4 value1, Vector4 value2)

-        {

-            value1.W -= value2.W;

-            value1.X -= value2.X;

-            value1.Y -= value2.Y;

-            value1.Z -= value2.Z;

-            return value1;

-        }

-

-        public static Vector4 operator *(Vector4 value1, Vector4 value2)

-        {

-            value1.W *= value2.W;

-            value1.X *= value2.X;

-            value1.Y *= value2.Y;

-            value1.Z *= value2.Z;

-            return value1;

-        }

-

-        public static Vector4 operator *(Vector4 value1, float scaleFactor)

-        {

-            value1.W *= scaleFactor;

-            value1.X *= scaleFactor;

-            value1.Y *= scaleFactor;

-            value1.Z *= scaleFactor;

-            return value1;

-        }

-

-        public static Vector4 operator *(float scaleFactor, Vector4 value1)

-        {

-            value1.W *= scaleFactor;

-            value1.X *= scaleFactor;

-            value1.Y *= scaleFactor;

-            value1.Z *= scaleFactor;

-            return value1;

-        }

-

-        public static Vector4 operator /(Vector4 value1, Vector4 value2)

-        {

-            value1.W /= value2.W;

-            value1.X /= value2.X;

-            value1.Y /= value2.Y;

-            value1.Z /= value2.Z;

-            return value1;

-        }

-

-        public static Vector4 operator /(Vector4 value1, float divider)

-        {

-            float factor = 1f / divider;

-            value1.W *= factor;

-            value1.X *= factor;

-            value1.Y *= factor;

-            value1.Z *= factor;

-            return value1;

-        }

-

-        #endregion Operators

-    }

-}

+#region License
+/*
+MIT License
+Copyright © 2006 The Mono.Xna Team
+
+All rights reserved.
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+#endregion License
+
+using System;
+using System.ComponentModel;
+using System.Text;
+using System.Runtime.InteropServices;
+
+namespace MonoXnaCompactMaths
+{
+    [Serializable]
+    [StructLayout(LayoutKind.Sequential)]
+    //[TypeConverter(typeof(Vector4Converter))]
+    public struct Vector4 : IEquatable<Vector4>
+    {
+        #region Private Fields
+
+        private static Vector4 zeroVector = new Vector4();
+        private static Vector4 unitVector = new Vector4(1f, 1f, 1f, 1f);
+        private static Vector4 unitXVector = new Vector4(1f, 0f, 0f, 0f);
+        private static Vector4 unitYVector = new Vector4(0f, 1f, 0f, 0f);
+        private static Vector4 unitZVector = new Vector4(0f, 0f, 1f, 0f);
+        private static Vector4 unitWVector = new Vector4(0f, 0f, 0f, 1f);
+
+        #endregion Private Fields
+
+
+        #region Public Fields
+
+        public float X;
+        public float Y;
+        public float Z;
+        public float W;
+
+        #endregion Public Fields
+
+
+        #region Properties
+
+        public static Vector4 Zero
+        {
+            get { return zeroVector; }
+        }
+
+        public static Vector4 One
+        {
+            get { return unitVector; }
+        }
+
+        public static Vector4 UnitX
+        {
+            get { return unitXVector; }
+        }
+
+        public static Vector4 UnitY
+        {
+            get { return unitYVector; }
+        }
+
+        public static Vector4 UnitZ
+        {
+            get { return unitZVector; }
+        }
+
+        public static Vector4 UnitW
+        {
+            get { return unitWVector; }
+        }
+
+        #endregion Properties
+
+
+        #region Constructors
+
+        public Vector4(float x, float y, float z, float w)
+        {
+            this.X = x;
+            this.Y = y;
+            this.Z = z;
+            this.W = w;
+        }
+
+        /*public Vector4(Vector2 value, float z, float w)
+        {
+            this.X = value.X;
+            this.Y = value.Y;
+            this.Z = z;
+            this.W = w;
+        }*/
+
+        public Vector4(Vector3 value, float w)
+        {
+            this.X = value.X;
+            this.Y = value.Y;
+            this.Z = value.Z;
+            this.W = w;
+        }
+
+        public Vector4(float value)
+        {
+            this.X = value;
+            this.Y = value;
+            this.Z = value;
+            this.W = value;
+        }
+
+        #endregion
+
+
+        #region Public Methods
+
+        public static Vector4 Add(Vector4 value1, Vector4 value2)
+        {
+            value1.W += value2.W;
+            value1.X += value2.X;
+            value1.Y += value2.Y;
+            value1.Z += value2.Z;
+            return value1;
+        }
+
+        public static void Add(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
+        {
+            result.W = value1.W + value2.W;
+            result.X = value1.X + value2.X;
+            result.Y = value1.Y + value2.Y;
+            result.Z = value1.Z + value2.Z;
+        }
+
+        /*public static Vector4 Barycentric(Vector4 value1, Vector4 value2, Vector4 value3, float amount1, float amount2)
+        {
+            return new Vector4(
+                MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2),
+                MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2),
+                MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2),
+                MathHelper.Barycentric(value1.W, value2.W, value3.W, amount1, amount2));
+        }*/
+
+        /*public static void Barycentric(ref Vector4 value1, ref Vector4 value2, ref Vector4 value3, float amount1, float amount2, out Vector4 result)
+        {
+            result = new Vector4(
+                MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2),
+                MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2),
+                MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2),
+                MathHelper.Barycentric(value1.W, value2.W, value3.W, amount1, amount2));
+        }*/
+
+        /*public static Vector4 CatmullRom(Vector4 value1, Vector4 value2, Vector4 value3, Vector4 value4, float amount)
+        {
+            return new Vector4(
+                MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),
+                MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount),
+                MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount),
+                MathHelper.CatmullRom(value1.W, value2.W, value3.W, value4.W, amount));
+        }*/
+
+        /*public static void CatmullRom(ref Vector4 value1, ref Vector4 value2, ref Vector4 value3, ref Vector4 value4, float amount, out Vector4 result)
+        {
+            result = new Vector4(
+                MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),
+                MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount),
+                MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount),
+                MathHelper.CatmullRom(value1.W, value2.W, value3.W, value4.W, amount));
+        }*/
+
+        /*public static Vector4 Clamp(Vector4 value1, Vector4 min, Vector4 max)
+        {
+            return new Vector4(
+                MathHelper.Clamp(value1.X, min.X, max.X),
+                MathHelper.Clamp(value1.Y, min.Y, max.Y),
+                MathHelper.Clamp(value1.Z, min.Z, max.Z),
+                MathHelper.Clamp(value1.W, min.W, max.W));
+        }*/
+
+        /*public static void Clamp(ref Vector4 value1, ref Vector4 min, ref Vector4 max, out Vector4 result)
+        {
+            result = new Vector4(
+                MathHelper.Clamp(value1.X, min.X, max.X),
+                MathHelper.Clamp(value1.Y, min.Y, max.Y),
+                MathHelper.Clamp(value1.Z, min.Z, max.Z),
+                MathHelper.Clamp(value1.W, min.W, max.W));
+        }*/
+
+        public static float Distance(Vector4 value1, Vector4 value2)
+        {
+            return (float)Math.Sqrt(DistanceSquared(value1, value2));
+        }
+
+        public static void Distance(ref Vector4 value1, ref Vector4 value2, out float result)
+        {
+            result = (float)Math.Sqrt(DistanceSquared(value1, value2));
+        }
+
+        public static float DistanceSquared(Vector4 value1, Vector4 value2)
+        {
+            float result;
+            DistanceSquared(ref value1, ref value2, out result);
+            return result;
+        }
+
+        public static void DistanceSquared(ref Vector4 value1, ref Vector4 value2, out float result)
+        {
+            result = (value1.W - value2.W) * (value1.W - value2.W) +
+                     (value1.X - value2.X) * (value1.X - value2.X) +
+                     (value1.Y - value2.Y) * (value1.Y - value2.Y) +
+                     (value1.Z - value2.Z) * (value1.Z - value2.Z);
+        }
+
+        public static Vector4 Divide(Vector4 value1, Vector4 value2)
+        {
+            value1.W /= value2.W;
+            value1.X /= value2.X;
+            value1.Y /= value2.Y;
+            value1.Z /= value2.Z;
+            return value1;
+        }
+
+        public static Vector4 Divide(Vector4 value1, float divider)
+        {
+            float factor = 1f / divider;
+            value1.W *= factor;
+            value1.X *= factor;
+            value1.Y *= factor;
+            value1.Z *= factor;
+            return value1;
+        }
+
+        public static void Divide(ref Vector4 value1, float divider, out Vector4 result)
+        {
+            float factor = 1f / divider;
+            result.W = value1.W * factor;
+            result.X = value1.X * factor;
+            result.Y = value1.Y * factor;
+            result.Z = value1.Z * factor;
+        }
+
+        public static void Divide(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
+        {
+            result.W = value1.W / value2.W;
+            result.X = value1.X / value2.X;
+            result.Y = value1.Y / value2.Y;
+            result.Z = value1.Z / value2.Z;
+        }
+
+        public static float Dot(Vector4 vector1, Vector4 vector2)
+        {
+            return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z + vector1.W * vector2.W;
+        }
+
+        public static void Dot(ref Vector4 vector1, ref Vector4 vector2, out float result)
+        {
+            result = vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z + vector1.W * vector2.W;
+        }
+
+        public override bool Equals(object obj)
+        {
+            return (obj is Vector4) ? this == (Vector4)obj : false;
+        }
+
+        public bool Equals(Vector4 other)
+        {
+            return this.W == other.W
+                && this.X == other.X
+                && this.Y == other.Y
+                && this.Z == other.Z;
+        }
+
+        public override int GetHashCode()
+        {
+            return (int)(this.W + this.X + this.Y + this.Y);
+        }
+
+        /*public static Vector4 Hermite(Vector4 value1, Vector4 tangent1, Vector4 value2, Vector4 tangent2, float amount)
+        {
+            Vector4 result = new Vector4();
+            Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result);
+            return result;
+        }*/
+
+        /*public static void Hermite(ref Vector4 value1, ref Vector4 tangent1, ref Vector4 value2, ref Vector4 tangent2, float amount, out Vector4 result)
+        {
+            result.W = MathHelper.Hermite(value1.W, tangent1.W, value2.W, tangent2.W, amount);
+            result.X = MathHelper.Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount);
+            result.Y = MathHelper.Hermite(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount);
+            result.Z = MathHelper.Hermite(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount);
+        }*/
+
+        public float Length()
+        {
+            float result;
+            DistanceSquared(ref this, ref zeroVector, out result);
+            return (float)Math.Sqrt(result);
+        }
+
+        public float LengthSquared()
+        {
+            float result;
+            DistanceSquared(ref this, ref zeroVector, out result);
+            return result;
+        }
+
+        /*public static Vector4 Lerp(Vector4 value1, Vector4 value2, float amount)
+        {
+            return new Vector4(
+                MathHelper.Lerp(value1.X, value2.X, amount),
+                MathHelper.Lerp(value1.Y, value2.Y, amount),
+                MathHelper.Lerp(value1.Z, value2.Z, amount),
+                MathHelper.Lerp(value1.W, value2.W, amount));
+        }*/
+
+        /*public static void Lerp(ref Vector4 value1, ref Vector4 value2, float amount, out Vector4 result)
+        {
+            result = new Vector4(
+                MathHelper.Lerp(value1.X, value2.X, amount),
+                MathHelper.Lerp(value1.Y, value2.Y, amount),
+                MathHelper.Lerp(value1.Z, value2.Z, amount),
+                MathHelper.Lerp(value1.W, value2.W, amount));
+        }*/
+
+        /*public static Vector4 Max(Vector4 value1, Vector4 value2)
+        {
+            return new Vector4(
+               MathHelper.Max(value1.X, value2.X),
+               MathHelper.Max(value1.Y, value2.Y),
+               MathHelper.Max(value1.Z, value2.Z),
+               MathHelper.Max(value1.W, value2.W));
+        }*/
+
+        /*public static void Max(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
+        {
+            result = new Vector4(
+               MathHelper.Max(value1.X, value2.X),
+               MathHelper.Max(value1.Y, value2.Y),
+               MathHelper.Max(value1.Z, value2.Z),
+               MathHelper.Max(value1.W, value2.W));
+        }*/
+
+        /*public static Vector4 Min(Vector4 value1, Vector4 value2)
+        {
+            return new Vector4(
+               MathHelper.Min(value1.X, value2.X),
+               MathHelper.Min(value1.Y, value2.Y),
+               MathHelper.Min(value1.Z, value2.Z),
+               MathHelper.Min(value1.W, value2.W));
+        }*/
+
+        /*public static void Min(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
+        {
+            result = new Vector4(
+               MathHelper.Min(value1.X, value2.X),
+               MathHelper.Min(value1.Y, value2.Y),
+               MathHelper.Min(value1.Z, value2.Z),
+               MathHelper.Min(value1.W, value2.W));
+        }*/
+
+        public static Vector4 Multiply(Vector4 value1, Vector4 value2)
+        {
+            value1.W *= value2.W;
+            value1.X *= value2.X;
+            value1.Y *= value2.Y;
+            value1.Z *= value2.Z;
+            return value1;
+        }
+
+        public static Vector4 Multiply(Vector4 value1, float scaleFactor)
+        {
+            value1.W *= scaleFactor;
+            value1.X *= scaleFactor;
+            value1.Y *= scaleFactor;
+            value1.Z *= scaleFactor;
+            return value1;
+        }
+
+        public static void Multiply(ref Vector4 value1, float scaleFactor, out Vector4 result)
+        {
+            result.W = value1.W * scaleFactor;
+            result.X = value1.X * scaleFactor;
+            result.Y = value1.Y * scaleFactor;
+            result.Z = value1.Z * scaleFactor;
+        }
+
+        public static void Multiply(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
+        {
+            result.W = value1.W * value2.W;
+            result.X = value1.X * value2.X;
+            result.Y = value1.Y * value2.Y;
+            result.Z = value1.Z * value2.Z;
+        }
+
+        public static Vector4 Negate(Vector4 value)
+        {
+            value = new Vector4(-value.X, -value.Y, -value.Z, -value.W);
+            return value;
+        }
+
+        public static void Negate(ref Vector4 value, out Vector4 result)
+        {
+            result = new Vector4(-value.X, -value.Y, -value.Z,-value.W);
+        }
+
+        public void Normalize()
+        {
+            Normalize(ref this, out this);
+        }
+
+        public static Vector4 Normalize(Vector4 vector)
+        {
+            Normalize(ref vector, out vector);
+            return vector;
+        }
+
+        public static void Normalize(ref Vector4 vector, out Vector4 result)
+        {
+            float factor;
+            DistanceSquared(ref vector, ref zeroVector, out factor);
+            factor = 1f / (float)Math.Sqrt(factor);
+
+            result.W = vector.W * factor;
+            result.X = vector.X * factor;
+            result.Y = vector.Y * factor;
+            result.Z = vector.Z * factor;
+        }
+
+        /*public static Vector4 SmoothStep(Vector4 value1, Vector4 value2, float amount)
+        {
+            return new Vector4(
+                MathHelper.SmoothStep(value1.X, value2.X, amount),
+                MathHelper.SmoothStep(value1.Y, value2.Y, amount),
+                MathHelper.SmoothStep(value1.Z, value2.Z, amount),
+                MathHelper.SmoothStep(value1.W, value2.W, amount));
+        }*/
+
+        /*public static void SmoothStep(ref Vector4 value1, ref Vector4 value2, float amount, out Vector4 result)
+        {
+            result = new Vector4(
+                MathHelper.SmoothStep(value1.X, value2.X, amount),
+                MathHelper.SmoothStep(value1.Y, value2.Y, amount),
+                MathHelper.SmoothStep(value1.Z, value2.Z, amount),
+                MathHelper.SmoothStep(value1.W, value2.W, amount));
+        }*/
+
+        public static Vector4 Subtract(Vector4 value1, Vector4 value2)
+        {
+            value1.W -= value2.W;
+            value1.X -= value2.X;
+            value1.Y -= value2.Y;
+            value1.Z -= value2.Z;
+            return value1;
+        }
+
+        public static void Subtract(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
+        {
+            result.W = value1.W - value2.W;
+            result.X = value1.X - value2.X;
+            result.Y = value1.Y - value2.Y;
+            result.Z = value1.Z - value2.Z;
+        }
+
+        /*public static Vector4 Transform(Vector2 position, Matrix matrix)
+        {
+            Vector4 result;
+            Transform(ref position, ref matrix, out result);
+            return result;
+        }*/
+
+        public static Vector4 Transform(Vector3 position, Matrix matrix)
+        {
+            Vector4 result;
+            Transform(ref position, ref matrix, out result);
+            return result;
+        }
+
+        public static Vector4 Transform(Vector4 vector, Matrix matrix)
+        {
+            Transform(ref vector, ref matrix, out vector);
+            return vector;
+        }
+
+        /*public static void Transform(ref Vector2 position, ref Matrix matrix, out Vector4 result)
+        {
+            result = new Vector4((position.X * matrix.M11) + (position.Y * matrix.M21) + matrix.M41,
+                                 (position.X * matrix.M12) + (position.Y * matrix.M22) + matrix.M42,
+                                 (position.X * matrix.M13) + (position.Y * matrix.M23) + matrix.M43,
+                                 (position.X * matrix.M14) + (position.Y * matrix.M24) + matrix.M44);
+        }*/
+
+        public static void Transform(ref Vector3 position, ref Matrix matrix, out Vector4 result)
+        {
+            result = new Vector4((position.X * matrix.M11) + (position.Y * matrix.M21) + (position.Z * matrix.M31) + matrix.M41,
+                                 (position.X * matrix.M12) + (position.Y * matrix.M22) + (position.Z * matrix.M32) + matrix.M42,
+                                 (position.X * matrix.M13) + (position.Y * matrix.M23) + (position.Z * matrix.M33) + matrix.M43,
+                                 (position.X * matrix.M14) + (position.Y * matrix.M24) + (position.Z * matrix.M34) + matrix.M44);
+        }
+
+        public static void Transform(ref Vector4 vector, ref Matrix matrix, out Vector4 result)
+        {
+            result = new Vector4((vector.X * matrix.M11) + (vector.Y * matrix.M21) + (vector.Z * matrix.M31) + (vector.W * matrix.M41),
+                                 (vector.X * matrix.M12) + (vector.Y * matrix.M22) + (vector.Z * matrix.M32) + (vector.W * matrix.M42),
+                                 (vector.X * matrix.M13) + (vector.Y * matrix.M23) + (vector.Z * matrix.M33) + (vector.W * matrix.M43),
+                                 (vector.X * matrix.M14) + (vector.Y * matrix.M24) + (vector.Z * matrix.M34) + (vector.W * matrix.M44));
+        }
+
+        public override string ToString()
+        {
+            StringBuilder sb = new StringBuilder(32);
+            sb.Append("{X:");
+            sb.Append(this.X);
+            sb.Append(" Y:");
+            sb.Append(this.Y);
+            sb.Append(" Z:");
+            sb.Append(this.Z);
+            sb.Append(" W:");
+            sb.Append(this.W);
+            sb.Append("}");
+            return sb.ToString();
+        }
+
+        #endregion Public Methods
+
+
+        #region Operators
+
+        public static Vector4 operator -(Vector4 value)
+        {
+            return new Vector4(-value.X, -value.Y, -value.Z, -value.W);
+        }
+
+        public static bool operator ==(Vector4 value1, Vector4 value2)
+        {
+            return value1.W == value2.W
+                && value1.X == value2.X
+                && value1.Y == value2.Y
+                && value1.Z == value2.Z;
+        }
+
+        public static bool operator !=(Vector4 value1, Vector4 value2)
+        {
+            return !(value1 == value2);
+        }
+
+        public static Vector4 operator +(Vector4 value1, Vector4 value2)
+        {
+            value1.W += value2.W;
+            value1.X += value2.X;
+            value1.Y += value2.Y;
+            value1.Z += value2.Z;
+            return value1;
+        }
+
+        public static Vector4 operator -(Vector4 value1, Vector4 value2)
+        {
+            value1.W -= value2.W;
+            value1.X -= value2.X;
+            value1.Y -= value2.Y;
+            value1.Z -= value2.Z;
+            return value1;
+        }
+
+        public static Vector4 operator *(Vector4 value1, Vector4 value2)
+        {
+            value1.W *= value2.W;
+            value1.X *= value2.X;
+            value1.Y *= value2.Y;
+            value1.Z *= value2.Z;
+            return value1;
+        }
+
+        public static Vector4 operator *(Vector4 value1, float scaleFactor)
+        {
+            value1.W *= scaleFactor;
+            value1.X *= scaleFactor;
+            value1.Y *= scaleFactor;
+            value1.Z *= scaleFactor;
+            return value1;
+        }
+
+        public static Vector4 operator *(float scaleFactor, Vector4 value1)
+        {
+            value1.W *= scaleFactor;
+            value1.X *= scaleFactor;
+            value1.Y *= scaleFactor;
+            value1.Z *= scaleFactor;
+            return value1;
+        }
+
+        public static Vector4 operator /(Vector4 value1, Vector4 value2)
+        {
+            value1.W /= value2.W;
+            value1.X /= value2.X;
+            value1.Y /= value2.Y;
+            value1.Z /= value2.Z;
+            return value1;
+        }
+
+        public static Vector4 operator /(Vector4 value1, float divider)
+        {
+            float factor = 1f / divider;
+            value1.W *= factor;
+            value1.X *= factor;
+            value1.Y *= factor;
+            value1.Z *= factor;
+            return value1;
+        }
+
+        #endregion Operators
+    }
+}