Stage 1 of adding Darok's bulletX plugin: adding the ModifiedBulletX project (which is based on the BulletXNA project, but modified to not use XNA).

ThirdPartyLicenses/Bullet for Xna (ModifiedBulletX).txt

@@ -0,0 +1,19 @@
+  Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
+  Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
+
+  This software is provided 'as-is', without any express or implied
+  warranty.  In no event will the authors be held liable for any damages
+  arising from the use of this software.
+
+  Permission is granted to anyone to use this software for any purpose,
+  including commercial applications, and to alter it and redistribute it
+  freely, subject to the following restrictions:
+
+  1. The origin of this software must not be misrepresented; you must not
+     claim that you wrote the original software. If you use this software
+     in a product, an acknowledgment in the product documentation would be
+     appreciated but is not required.
+  2. Altered source versions must be plainly marked as such, and must not be
+     misrepresented as being the original software.
+  3. This notice may not be removed or altered from any source distribution.
+  

ThirdPartyLicenses/Mono.Xna (MonoXnaCompactMaths).txt

@@ -0,0 +1,22 @@
+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.

libraries/ModifiedBulletX/ModifiedBulletX.sln

@@ -0,0 +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

libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs

@@ -0,0 +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);
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs

@@ -0,0 +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];
+        }
+    }
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs

@@ -0,0 +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,
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs

@@ -0,0 +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);
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs

@@ -0,0 +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,
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs

@@ -0,0 +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);
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs

@@ -0,0 +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; } }
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs

@@ -0,0 +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; } }
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs

@@ -0,0 +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);
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs

@@ -0,0 +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; }
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs

@@ -0,0 +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);
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs

@@ -0,0 +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
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs

@@ -0,0 +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));
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs

@@ -0,0 +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; } }
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs

@@ -0,0 +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);
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs

@@ -0,0 +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;
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs

@@ -0,0 +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;
+					}
+				}
+			}
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs

@@ -0,0 +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;
+			}
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs

@@ -0,0 +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
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs

@@ -0,0 +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;
+			}
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs

@@ -0,0 +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);
+			}
+		};
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs

@@ -0,0 +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;
+				}
+			}
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs

@@ -0,0 +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
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs

@@ -0,0 +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
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs

@@ -0,0 +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);
+			}
+		};
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs

@@ -0,0 +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;
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs

@@ -0,0 +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);
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs

@@ -0,0 +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
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs

@@ -0,0 +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);
+			}
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs

@@ -0,0 +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
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs

@@ -0,0 +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);
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs

@@ -0,0 +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; } }
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs

@@ -0,0 +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;
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs

@@ -0,0 +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);
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs

@@ -0,0 +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";
+            }
+        }
+    }
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs

@@ -0,0 +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);
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs

@@ -0,0 +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;
+			}
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs

@@ -0,0 +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);
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs

@@ -0,0 +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;
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs

@@ -0,0 +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;
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs

@@ -0,0 +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);
+			}
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs

@@ -0,0 +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;
+			}
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs

@@ -0,0 +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;
+			}
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs

@@ -0,0 +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;
+			}
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs

@@ -0,0 +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;
+			}
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs

@@ -0,0 +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);
+        }
+    }
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs

@@ -0,0 +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);
+			}
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs

@@ -0,0 +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);
+    }
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs

@@ -0,0 +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];
+				}
+			}
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs

@@ -0,0 +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);
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs

@@ -0,0 +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);
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs

@@ -0,0 +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);
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs

@@ -0,0 +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);
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs

@@ -0,0 +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; } }
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs

@@ -0,0 +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));
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs

@@ -0,0 +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;
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs

@@ -0,0 +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);
+        }
+    }
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs

@@ -0,0 +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; }
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs

@@ -0,0 +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
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs

@@ -0,0 +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
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs

@@ -0,0 +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);
+    }
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs

@@ -0,0 +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.");
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs

@@ -0,0 +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)
+        {
+
+        }
+    }
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs

@@ -0,0 +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);
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs

@@ -0,0 +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;
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs

@@ -0,0 +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;
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs

@@ -0,0 +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);
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs

@@ -0,0 +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.");
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs

@@ -0,0 +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
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs

@@ -0,0 +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);
+			}
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs

@@ -0,0 +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;
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs

@@ -0,0 +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;
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs

@@ -0,0 +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);
+				}
+			}
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs

@@ -0,0 +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
+		);
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs

@@ -0,0 +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;}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs

@@ -0,0 +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; } }
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs

@@ -0,0 +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
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs

@@ -0,0 +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; }
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs

@@ -0,0 +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;
+			}
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs

@@ -0,0 +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
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs

@@ -0,0 +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
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs

@@ -0,0 +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;
+		}
+    }
+}

libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs

@@ -0,0 +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;
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs

@@ -0,0 +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; } }
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs

@@ -0,0 +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;
+			}
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs

@@ -0,0 +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)
+		{
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs

@@ -0,0 +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);
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs

@@ -0,0 +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;
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs

@@ -0,0 +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);
+			}
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs

@@ -0,0 +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; } }
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs

@@ -0,0 +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;
+		//}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs

@@ -0,0 +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;
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs

@@ -0,0 +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,
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs

@@ -0,0 +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;
+		}
+	}
+}

libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs

@@ -0,0 +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
+	}
+}