OpenSim
/
OpenSimMirror
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

libraries moved to opensim-libs, a new repository

afrisby
dan miller 2007-10-21 08:36:32 +00:00
parent 6119eaed85
commit 2f8d7092bc
1452 changed files with 0 additions and 620304 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/ModifiedBulletX/ModifiedBulletX.sln
View File

@ -1,42 +0,0 @@

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

BIN
libraries/ModifiedBulletX/ModifiedBulletX.suo
View File

Binary file not shown.

68
libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs
View File

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

BIN
libraries/ModifiedBulletX/ModifiedBulletX/BulletX.snk
View File

Binary file not shown.

623
libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs
View File

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

68
libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs
View File

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

113
libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs
View File

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

91
libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs
View File

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

51
libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs
View File

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

42
libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs
View File

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

54
libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs
View File

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

36
libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs
View File

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

42
libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs
View File

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

33
libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs
View File

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

159
libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs
View File

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

128
libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs
View File

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

46
libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs
View File

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

59
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs
View File

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

40
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs
View File

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

280
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs
View File

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

163
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs
View File

@ -1,163 +0,0 @@
/*
Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any damages
arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software
in a product, an acknowledgment in the product documentation would be
appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
using System;
using System.Collections.Generic;
using System.Text;
using MonoXnaCompactMaths;
namespace XnaDevRu.BulletX
{
public enum ActivationState
{
Nothing = 0,
Active,
IslandSleeping,
WantsDeactivation,
DisableDeactivation,
DisableSimulation,
}
public enum CollisionOptions
{
StaticObject = 1,
KinematicObject = 2,
NoContactResponse = 4,
CustomMaterialCallback = 8,//this allows per-triangle material (friction/restitution)
}
/// <summary>
/// btCollisionObject can be used to manage collision detection objects.
/// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy.
/// They can be added to the btCollisionWorld.
/// </summary>
public class CollisionObject
{
protected Matrix _worldTransform;
private BroadphaseProxy _broadphase;
private CollisionShape _collisionShape;
//m_interpolationWorldTransform is used for CCD and interpolation
//it can be either previous or future (predicted) transform
private Matrix _interpolationWorldTransform;
private CollisionOptions _collisionFlags;
private int _islandTag;
private ActivationState _activationState;
private float _deactivationTime;
private float _friction;
private float _restitution;
//users can point to their objects, m_userPointer is not used by Bullet
private object _userData;
//m_internalOwner one is used by optional Bullet high level interface
private object _internalOwner;
//time of impact calculation
private float _hitFraction;
//Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
private float _ccdSweptSphereRadius;
// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionTreshold
private float _ccdSquareMotionThreshold;
//those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities)
//without destroying the continuous interpolated motion (which uses this interpolation velocities)
private Vector3 _interpolationLinearVelocity;
private Vector3 _interpolationAngularVelocity;
private int _companionID;
public CollisionObject()
{
_activationState = ActivationState.Active;
_hitFraction = 1;
}
public bool IsStaticObject { get { return (_collisionFlags & CollisionOptions.StaticObject) != 0; } }
public bool IsKinematicObject { get { return (_collisionFlags & CollisionOptions.KinematicObject) != 0; } }
public bool IsStaticOrKinematicObject { get { return (_collisionFlags & (CollisionOptions.KinematicObject | CollisionOptions.StaticObject)) != 0; } }
public bool HasContactResponse { get { return (_collisionFlags & CollisionOptions.NoContactResponse) == 0; } }
public bool MergesSimulationIslands
{
get
{
//static objects, kinematic and object without contact response don't merge islands
return (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject | CollisionOptions.NoContactResponse)) == 0;
}
}
public ActivationState ActivationState
{
get { return _activationState; }
set
{
if ((_activationState != ActivationState.DisableDeactivation) && (_activationState != ActivationState.DisableSimulation))
_activationState = value;
}
}
public bool IsActive { get { return ((ActivationState != ActivationState.IslandSleeping) && (ActivationState != ActivationState.DisableSimulation)); } }
public float Restitution { get { return _restitution; } set { _restitution = value; } }
public float Friction { get { return _friction; } set { _friction = value; } }
public CollisionShape CollisionShape { get { return _collisionShape; } set { _collisionShape = value; } }
public float DeactivationTime { get { return _deactivationTime; } set { _deactivationTime = value; } }
public object Owner { get { return _internalOwner; } protected set { _internalOwner = value; } }
public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } }
public BroadphaseProxy Broadphase { get { return _broadphase; } set { _broadphase = value; } }
public Matrix InterpolationWorldTransform { get { return _interpolationWorldTransform; } set { _interpolationWorldTransform = value; } }
public Vector3 InterpolationLinearVelocity { get { return _interpolationLinearVelocity; } protected set { _interpolationLinearVelocity = value; } }
public Vector3 InterpolationAngularVelocity { get { return _interpolationAngularVelocity; } protected set { _interpolationAngularVelocity = value; } }
public int IslandTag { get { return _islandTag; } set { _islandTag = value; } }
public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } }
public CollisionOptions CollisionFlags { get { return _collisionFlags; } set { _collisionFlags = value; } }
//Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm
public float CcdSweptSphereRadius { get { return _ccdSweptSphereRadius; } set { _ccdSweptSphereRadius = value; } }
// Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold
public float CcdSquareMotionThreshold { get { return _ccdSquareMotionThreshold; } set { _ccdSquareMotionThreshold = value; } }
//users can point to their objects, userPointer is not used by Bullet
public object UserData { get { return _userData; } set { _userData = value; } }
public int CompanionID { get { return _companionID; } set { _companionID = value; } }
public void ForceActivationState(ActivationState newState)
{
_activationState = newState;
}
public void Activate()
{
Activate(false);
}
public void Activate(bool forceActivation)
{
if (forceActivation || (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject)) == 0)
{
ActivationState = ActivationState.Active;
_deactivationTime = 0;
}
}
}
}

48
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs
View File

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

358
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs
View File

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

157
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs
View File

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

189
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs
View File

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

193
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs
View File

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

130
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs
View File

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

52
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs
View File

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

147
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs
View File

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

304
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs
View File

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

270
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs
View File

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

104
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs
View File

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

100
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs
View File

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

214
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs
View File

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

151
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs
View File

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

215
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs
View File

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

316
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs
View File

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

83
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs
View File

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

148
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs
View File

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

183
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs
View File

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

55
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs
View File

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

208
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs
View File

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

184
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs
View File

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

141
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs
View File

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

185
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs
View File

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

136
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs
View File

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

100
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs
View File

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

100
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs
View File

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

80
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs
View File

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

55
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs
View File

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

360
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/HeightfieldTerrainShape.cs
View File

@ -1,360 +0,0 @@
/*
* WARNING!: this class is not in the original BulletX
* By the way it's based on the Bullet btHeightfieldTerrainShape:
* http://www.continuousphysics.com/Bullet/BulletFull/classbtHeightfieldTerrainShape.html
*****************************************************************************************
* 3RD PARTY LICENSE. The next it's the original 3rd party lincense of Bullet:
* ----------------------------------------------------------------------------
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice 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 HeightfieldTerrainShape : ConcaveShape
{
private Vector3 _localAabbMin;
private Vector3 _localAabbMax;
private Vector3 _localScaling = new Vector3(1f,1f,1f);
private int _width;
private int _length;
private float[] _heightfieldData;
private float _maxHeight;
private int _upAxis;
private bool _useFloatData;
private bool _flipQuadEdges;
private bool _useDiamondSubdivision = false;
private float _defaultCollisionMargin = 0.6f;
public HeightfieldTerrainShape(int width, int length, float[] heightfieldData, float maxHeight,
int upAxis, bool useFloatData, bool flipQuadEdges)
{
_width = width;
_length = length;
_heightfieldData = heightfieldData;
_maxHeight = maxHeight;
_upAxis = upAxis;
_useFloatData = useFloatData;
_flipQuadEdges = flipQuadEdges;
this.Margin = _defaultCollisionMargin;
float quantizationMargin = 1f;
//enlarge the AABB to avoid division by zero when initializing the quantization value
Vector3 clampValue = new Vector3(quantizationMargin, quantizationMargin, quantizationMargin);
Vector3 halfExtents = new Vector3(0, 0, 0);
switch (_upAxis)
{
case 0:
halfExtents.X = _maxHeight;
halfExtents.Y = _width;
halfExtents.Z = _length;
break;
case 1:
halfExtents.X = _width;
halfExtents.Y = _maxHeight;
halfExtents.Z = _length;
break;
case 2:
halfExtents.X = _width;
halfExtents.Y = _length;
halfExtents.Z = _maxHeight;
break;
default:
//need to get valid _upAxis
//btAssert(0);
throw new Exception("HeightfieldTerrainShape: need to get valid _upAxis");
}
halfExtents *= 0.5f;
_localAabbMin = -halfExtents - clampValue;
_localAabbMax = halfExtents + clampValue;
//Vector3 aabbSize = new Vector3();
//aabbSize = m_localAabbMax - m_localAabbMin;
}
protected Vector3 LocalAabbMin
{ get { return _localAabbMin; } set { _localAabbMin = value; } }
protected Vector3 LocalAabbMax
{ get { return _localAabbMax; } set { _localAabbMax = value; } }
public override string Name
{
get
{
return "HeightfieldTerrain";
}
}
public override Vector3 LocalScaling
{
get
{
return _localScaling;
}
set
{
_localScaling = value;
}
}
public override float Margin
{
get
{
return base.Margin;
}
set
{
base.Margin = value;
}
}
public override BroadphaseNativeTypes ShapeType
{
get { return BroadphaseNativeTypes.Terrain; }
}
public Vector3 HalfExtents
{
get
{
Vector3 halfExtents = new Vector3();
switch (_upAxis)
{
case 0:
halfExtents.X = 2f;//_maxHeight;
halfExtents.Y = _width;
halfExtents.Z = _length;
break;
case 1:
halfExtents.X = _width;
halfExtents.Y = 2f;// _maxHeight;
halfExtents.Z = _length;
break;
case 2:
halfExtents.X = _width;
halfExtents.Y = _length;
halfExtents.Z = 2f;// _maxHeight;
break;
default:
//need to get valid m_upAxis
//btAssert(0);
throw new Exception("HeightfieldTerrainShape: need to get valid _upAxis");
//break;
}
halfExtents *= 0.5f;
return halfExtents;
}
}
public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
{
//(void)callback;
//(void)aabbMax;
//(void)aabbMin;
//quantize the aabbMin and aabbMax, and adjust the start/end ranges
int[] quantizedAabbMin = new int[3];
int[] quantizedAabbMax = new int[3];
Vector3 localAabbMin = aabbMin * new Vector3(1f/_localScaling.X,1f/_localScaling.Y,1f/_localScaling.Z );
Vector3 localAabbMax = aabbMax * new Vector3(1f/_localScaling.X,1f/_localScaling.Y,1f/_localScaling.Z);
quantizeWithClamp(ref quantizedAabbMin, localAabbMin);
quantizeWithClamp(ref quantizedAabbMax, localAabbMax);
int startX=0;
int endX=_width-1;
int startJ=0;
int endJ=_length-1;
switch(_upAxis)
{
case 0:
quantizedAabbMin[1]+=_width/2-1;
quantizedAabbMax[1]+=_width/2+1;
quantizedAabbMin[2]+=_length/2-1;
quantizedAabbMax[2]+=_length/2+1;
if (quantizedAabbMin[1]>startX)
startX = quantizedAabbMin[1];
if (quantizedAabbMax[1]<endX)
endX = quantizedAabbMax[1];
if (quantizedAabbMin[2]>startJ)
startJ = quantizedAabbMin[2];
if (quantizedAabbMax[2]<endJ)
endJ = quantizedAabbMax[2];
break;
case 1:
quantizedAabbMin[0]+=_width/2-1;
quantizedAabbMax[0]+=_width/2+1;
quantizedAabbMin[2]+=_length/2-1;
quantizedAabbMax[2]+=_length/2+1;
if (quantizedAabbMin[0]>startX)
startX = quantizedAabbMin[0];
if (quantizedAabbMax[0]<endX)
endX = quantizedAabbMax[0];
if (quantizedAabbMin[2]>startJ)
startJ = quantizedAabbMin[2];
if (quantizedAabbMax[2]<endJ)
endJ = quantizedAabbMax[2];
break;
case 2:
quantizedAabbMin[0]+=_width/2-1;
quantizedAabbMax[0]+=_width/2+1;
quantizedAabbMin[1]+=_length/2-1;
quantizedAabbMax[1]+=_length/2+1;
if (quantizedAabbMin[0]>startX)
startX = quantizedAabbMin[0];
if (quantizedAabbMax[0]<endX)
endX = quantizedAabbMax[0];
if (quantizedAabbMin[1]>startJ)
startJ = quantizedAabbMin[1];
if (quantizedAabbMax[1]<endJ)
endJ = quantizedAabbMax[1];
break;
default:
//need to get valid m_upAxis
throw new Exception("HeightfieldTerrainShape: need to get valid _upAxis");
//break;
}
for(int j=startJ; j<endJ; j++)
{
for(int x=startX; x<endX; x++)
{
Vector3[] vertices = new Vector3[3];
//if (m_flipQuadEdges || (m_useDiamondSubdivision && ((j + x) & 1)))
if (_flipQuadEdges || (_useDiamondSubdivision && (((j + x) & 1) > 0)))
{
//first triangle
getVertex(x,j,ref vertices[0]);
getVertex(x+1,j,ref vertices[1]);
getVertex(x+1,j+1,ref vertices[2]);
//callback->processTriangle(vertices,x,j);
callback.ProcessTriangle(vertices,x,j);
//second triangle
getVertex(x,j,ref vertices[0]);
getVertex(x+1,j+1,ref vertices[1]);
getVertex(x,j+1,ref vertices[2]);
//callback->processTriangle(vertices,x,j);
callback.ProcessTriangle(vertices, x, j);
}
else
{
//first triangle
getVertex(x,j,ref vertices[0]);
getVertex(x,j+1,ref vertices[1]);
getVertex(x+1,j,ref vertices[2]);
//callback->processTriangle(vertices,x,j);
callback.ProcessTriangle(vertices,x,j);
//second triangle
getVertex(x+1,j,ref vertices[0]);
getVertex(x,j+1,ref vertices[1]);
getVertex(x+1,j+1,ref vertices[2]);
//callback->processTriangle(vertices,x,j);
callback.ProcessTriangle(vertices,x,j);
}
}
}
}
public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
{
//aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
//aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
Vector3 halfExtents = (_localAabbMax - _localAabbMin) * _localScaling * 0.5f;
Vector3 center = t.Translation;
Vector3 extent = new Vector3(Math.Abs(halfExtents.X), Math.Abs(halfExtents.Y), Math.Abs(halfExtents.Z));
extent += new Vector3(Margin, Margin, Margin);
aabbMin = center - extent;
aabbMax = center + extent;
}
public override void CalculateLocalInertia(float mass, out Vector3 inertia)
{
//moving concave objects not supported
inertia = new Vector3();
}
public float getHeightFieldValue(int x,int y)
{
float val = 0f;
if (_useFloatData)
{
val = _heightfieldData[(y * _width) + x];
}
else
{
//assume unsigned short int
int heightFieldValue = (int)_heightfieldData[(y * _width) + x];
val = heightFieldValue * _maxHeight/65535f;
}
return val;
}
public void getVertex(int x,int y,ref Vector3 vertex)
{
if (x < 0) x = 0;
if (y < 0) y = 0;
if (x >= _width) x = _width - 1;
if (y >= _length) y = _length - 1;
float height = getHeightFieldValue(x,y);
switch(_upAxis)
{
case 0:
vertex.X = height;
vertex.Y = (- _width/2 ) + x;
vertex.Z = (- _length/2 ) + y;
break;
case 1:
vertex.X = (- _width/2 ) + x;
vertex.Y = height;
vertex.Z = (- _length/2 ) + y;
break;
case 2:
vertex.X = (- _width/2 ) + x;
vertex.Y = (- _length/2 ) + y;
vertex.Z = height;
break;
default:
//need to get valid m_upAxis
throw new Exception("HeightfieldTerrainShape: need to get valid _upAxis");
//break;
}
vertex *= _localScaling;
}
public void quantizeWithClamp(ref int[] _out,Vector3 point)
{
Vector3 clampedPoint = point;
MathHelper.SetMax(ref clampedPoint,_localAabbMin);
MathHelper.SetMin(ref clampedPoint, _localAabbMax);
Vector3 v = clampedPoint;
_out[0] = (int)(v.X);
_out[1] = (int)(v.Y);
_out[2] = (int)(v.Z);
//correct for
}
}
}

33
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs
View File

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

58
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs
View File

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

99
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs
View File

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

154
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs
View File

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

32
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs
View File

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

293
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs
View File

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

63
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs
View File

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

133
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs
View File

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

116
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs
View File

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

124
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs
View File

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

115
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs
View File

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

67
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs
View File

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

80
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs
View File

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

33
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs
View File

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

136
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs
View File

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

102
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs
View File

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

160
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs
View File

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

187
libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs
View File

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

199
libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs
View File

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

73
libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs
View File

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

117
libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs
View File

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

176
libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs
View File

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

633
libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs
View File

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

56
libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs
View File

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

101
libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs
View File

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

343
libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs
View File

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

42
libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs
View File

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

48
libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs
View File

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

78
libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs
View File

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

246
libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs
View File

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

272
libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs
View File

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

64
libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs
View File

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

142
libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs
View File

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

115
libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs
View File

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

643
libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs
View File

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

488
libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs
View File

@ -1,488 +0,0 @@
/*
Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any damages
arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software
in a product, an acknowledgment in the product documentation would be
appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
using System;
using System.Collections.Generic;
using System.Text;
using MonoXnaCompactMaths;
namespace XnaDevRu.BulletX.Dynamics
{
public delegate float ContactSolverFunc (RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo info);
public enum ContactSolverType
{
Default = 0,
TypeA,
TypeB,
User,
MaxContactSolverType,
}
public class ConstraintPersistentData
{
// total applied impulse during most recent frame
private float _appliedImpulse;
private float _previousAppliedImpulse;
private float _accumulatedTangentImpulse0;
private float _accumulatedTangentImpulse1;
private float _jacDiagABInv;
private float _jacDiagABInvTangentA;
private float _jacDiagABInvTangentB;
private int _persistentLifeTime;
private float _restitution;
private float _friction;
private float _penetration;
private Vector3 _frictionWorldTangentialA;
private Vector3 _frictionWorldTangentialB;
private Vector3 _frictionAngularComponent0A;
private Vector3 _frictionAngularComponent0B;
private Vector3 _frictionAngularComponent1A;
private Vector3 _frictionAngularComponent1B;
//some data doesn't need to be persistent over frames: todo: clean/reuse this
private Vector3 _angularComponentA;
private Vector3 _angularComponentB;
private ContactSolverFunc _contactSolverFunc;
private ContactSolverFunc _frictionSolverFunc;
public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } }
public float PreviousAppliedImpulse { get { return _previousAppliedImpulse; } set { _previousAppliedImpulse = value; } }
public float AccumulatedTangentImpulseA { get { return _accumulatedTangentImpulse0; } set { _accumulatedTangentImpulse0 = value; } }
public float AccumulatedTangentImpulseB { get { return _accumulatedTangentImpulse1; } set { _accumulatedTangentImpulse1 = value; } }
public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } }
public float JacDiagABInvTangentA { get { return _jacDiagABInvTangentA; } set { _jacDiagABInvTangentA = value; } }
public float JacDiagABInvTangentB { get { return _jacDiagABInvTangentB; } set { _jacDiagABInvTangentB = value; } }
public int PersistentLifeTime { get { return _persistentLifeTime; } set { _persistentLifeTime = value; } }
public float Restitution { get { return _restitution; } set { _restitution = value; } }
public float Friction { get { return _friction; } set { _friction = value; } }
public float Penetration { get { return _penetration; } set { _penetration = value; } }
public Vector3 FrictionWorldTangentialA { get { return _frictionWorldTangentialA; } set { _frictionWorldTangentialA = value; } }
public Vector3 FrictionWorldTangentialB { get { return _frictionWorldTangentialB; } set { _frictionWorldTangentialB = value; } }
public Vector3 FrictionAngularComponent0A { get { return _frictionAngularComponent0A; } set { _frictionAngularComponent0A = value; } }
public Vector3 FrictionAngularComponent0B { get { return _frictionAngularComponent0B; } set { _frictionAngularComponent0B = value; } }
public Vector3 FrictionAngularComponent1A { get { return _frictionAngularComponent1A; } set { _frictionAngularComponent1A = value; } }
public Vector3 FrictionAngularComponent1B { get { return _frictionAngularComponent1B; } set { _frictionAngularComponent1B = value; } }
public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } }
public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } }
public ContactSolverFunc ContactSolverFunc { get { return _contactSolverFunc; } set { _contactSolverFunc = value; } }
public ContactSolverFunc FrictionSolverFunc { get { return _frictionSolverFunc; } set { _frictionSolverFunc = value; } }
}
public static class ContactConstraint
{
private const int UseInternalApplyImpulse = 1;
/// <summary>
/// bilateral constraint between two dynamic objects
/// positive distance = separation, negative distance = penetration
/// </summary>
/// <param name="body1"></param>
/// <param name="pos1"></param>
/// <param name="body2"></param>
/// <param name="pos2"></param>
/// <param name="distance"></param>
/// <param name="normal"></param>
/// <param name="impulse"></param>
/// <param name="timeStep"></param>
public static void ResolveSingleBilateral(RigidBody bodyA, Vector3 posA,
RigidBody bodyB, Vector3 posB,
float distance, Vector3 normal, out float impulse, float timeStep)
{
float normalLenSqr = normal.LengthSquared();
if (Math.Abs(normalLenSqr) >= 1.1f)
throw new BulletException();
/*if (normalLenSqr > 1.1f)
{
impulse = 0f;
return;
}*/
Vector3 rel_pos1 = posA - bodyA.CenterOfMassPosition;
Vector3 rel_pos2 = posB - bodyB.CenterOfMassPosition;
//this jacobian entry could be re-used for all iterations
Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
JacobianEntry jac = new JacobianEntry(Matrix.Transpose(bodyA.CenterOfMassTransform),
Matrix.Transpose(bodyB.CenterOfMassTransform),
rel_pos1, rel_pos2, normal, bodyA.InvInertiaDiagLocal, bodyA.InverseMass,
bodyB.InvInertiaDiagLocal, bodyB.InverseMass);
float jacDiagAB = jac.Diagonal;
float jacDiagABInv = 1f / jacDiagAB;
float rel_vel = jac.GetRelativeVelocity(
bodyA.LinearVelocity,
Vector3.TransformNormal(bodyA.AngularVelocity, Matrix.Transpose(bodyA.CenterOfMassTransform)),
bodyB.LinearVelocity,
Vector3.TransformNormal(bodyB.AngularVelocity, Matrix.Transpose(bodyB.CenterOfMassTransform)));
float a;
a = jacDiagABInv;
rel_vel = Vector3.Dot(normal, vel);
float contactDamping = 0.2f;
float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
impulse = velocityImpulse;
}
/// <summary>
/// contact constraint resolution:
/// calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
/// positive distance = separation, negative distance = penetration
/// </summary>
/// <param name="body1"></param>
/// <param name="body2"></param>
/// <param name="contactPoint"></param>
/// <param name="info"></param>
/// <returns></returns>
public static float ResolveSingleCollision(RigidBody bodyA, RigidBody bodyB,
ManifoldPoint contactPoint, ContactSolverInfo solverInfo)
{
Vector3 pos1 = contactPoint.PositionWorldOnA;
Vector3 pos2 = contactPoint.PositionWorldOnB;
// printf("distance=%f\n",distance);
Vector3 normal = contactPoint.NormalWorldOnB;
Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition;
Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition;
Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
float rel_vel;
rel_vel = Vector3.Dot(normal, vel);
float Kfps = 1f / solverInfo.TimeStep;
//float damping = solverInfo.m_damping;
float Kerp = solverInfo.Erp;
float Kcor = Kerp * Kfps;
//printf("dist=%f\n",distance);
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
throw new BulletException();
float distance = cpd.Penetration;//contactPoint.getDistance();
//distance = 0.f;
float positionalError = Kcor * -distance;
//jacDiagABInv;
float velocityError = cpd.Restitution - rel_vel;// * damping;
float penetrationImpulse = positionalError * cpd.JacDiagABInv;
float velocityImpulse = velocityError * cpd.JacDiagABInv;
float normalImpulse = penetrationImpulse + velocityImpulse;
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
float oldNormalImpulse = cpd.AppliedImpulse;
float sum = oldNormalImpulse + normalImpulse;
cpd.AppliedImpulse = 0f > sum ? 0f : sum;
normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;
if (bodyA.InverseMass != 0)
{
bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);
}
if (bodyB.InverseMass != 0)
{
bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);
}
/*body1.applyImpulse(normal * (normalImpulse), rel_pos1);
body2.applyImpulse(-normal * (normalImpulse), rel_pos2);*/
return normalImpulse;
}
public static float ResolveSingleFriction(RigidBody bodyA, RigidBody bodyB,
ManifoldPoint contactPoint, ContactSolverInfo solverInfo)
{
Vector3 pos1 = contactPoint.PositionWorldOnA;
Vector3 pos2 = contactPoint.PositionWorldOnB;
Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition;
Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition;
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
throw new BulletException();
float combinedFriction = cpd.Friction;
float limit = cpd.AppliedImpulse * combinedFriction;
//friction
if (cpd.AppliedImpulse > 0)
{
//apply friction in the 2 tangential directions
// 1st tangent
Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
float j1, j2;
{
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);
// calculate j that moves us to zero relative velocity
j1 = -vrel * cpd.JacDiagABInvTangentA;
float oldTangentImpulse = cpd.AccumulatedTangentImpulseA;
cpd.AccumulatedTangentImpulseA = oldTangentImpulse + j1;
float atia = cpd.AccumulatedTangentImpulseA;
MathHelper.SetMin(ref atia, limit);
MathHelper.SetMax(ref atia, -limit);
cpd.AccumulatedTangentImpulseA = atia;
j1 = cpd.AccumulatedTangentImpulseA - oldTangentImpulse;
}
{
// 2nd tangent
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);
// calculate j that moves us to zero relative velocity
j2 = -vrel * cpd.JacDiagABInvTangentB;
float oldTangentImpulse = cpd.AccumulatedTangentImpulseB;
cpd.AccumulatedTangentImpulseB = oldTangentImpulse + j2;
float atib = cpd.AccumulatedTangentImpulseB;
MathHelper.SetMin(ref atib, limit);
MathHelper.SetMax(ref atib, -limit);
cpd.AccumulatedTangentImpulseB = atib;
j2 = cpd.AccumulatedTangentImpulseB - oldTangentImpulse;
}
if (bodyA.InverseMass != 0)
{
bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyA.InverseMass, cpd.FrictionAngularComponent0A, j1);
bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyA.InverseMass, cpd.FrictionAngularComponent1A, j2);
}
if (bodyB.InverseMass != 0)
{
bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyB.InverseMass, cpd.FrictionAngularComponent0B, -j1);
bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyB.InverseMass, cpd.FrictionAngularComponent1B, -j2);
}
}
return cpd.AppliedImpulse;
}
public static float ResolveSingleFrictionOriginal(
RigidBody bodyA,
RigidBody bodyB,
ManifoldPoint contactPoint,
ContactSolverInfo solverInfo)
{
Vector3 posA = contactPoint.PositionWorldOnA;
Vector3 posB = contactPoint.PositionWorldOnB;
Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
Vector3 relPosB = posB - bodyB.CenterOfMassPosition;
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
throw new BulletException();
float combinedFriction = cpd.Friction;
float limit = cpd.AppliedImpulse * combinedFriction;
//if (contactPoint.m_appliedImpulse>0.f)
//friction
{
//apply friction in the 2 tangential directions
{
// 1st tangent
Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel = velA - velB;
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);
// calculate j that moves us to zero relative velocity
float j = -vrel * cpd.JacDiagABInvTangentA;
float total = cpd.AccumulatedTangentImpulseA + j;
if (limit < total)
total = limit;
if (total < -limit)
total = -limit;
j = total - cpd.AccumulatedTangentImpulseA;
cpd.AccumulatedTangentImpulseA = total;
bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA);
bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB);
}
{
// 2nd tangent
Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel = velA - velB;
float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);
// calculate j that moves us to zero relative velocity
float j = -vrel * cpd.JacDiagABInvTangentB;
float total = cpd.AccumulatedTangentImpulseB + j;
if (limit < total)
total = limit;
if (total < -limit)
total = -limit;
j = total - cpd.AccumulatedTangentImpulseB;
cpd.AccumulatedTangentImpulseB = total;
bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA);
bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB);
}
}
return cpd.AppliedImpulse;
}
//velocity + friction
//response between two dynamic objects with friction
public static float ResolveSingleCollisionCombined(
RigidBody bodyA,
RigidBody bodyB,
ManifoldPoint contactPoint,
ContactSolverInfo solverInfo)
{
Vector3 posA = contactPoint.PositionWorldOnA;
Vector3 posB = contactPoint.PositionWorldOnB;
Vector3 normal = contactPoint.NormalWorldOnB;
Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
Vector3 relPosB = posB - bodyB.CenterOfMassPosition;
Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel = velA - velB;
float relVel;
relVel = Vector3.Dot(normal, vel);
float Kfps = 1f / solverInfo.TimeStep;
//float damping = solverInfo.m_damping;
float Kerp = solverInfo.Erp;
float Kcor = Kerp * Kfps;
ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
if (cpd == null)
throw new BulletException();
float distance = cpd.Penetration;
float positionalError = Kcor * -distance;
float velocityError = cpd.Restitution - relVel;// * damping;
float penetrationImpulse = positionalError * cpd.JacDiagABInv;
float velocityImpulse = velocityError * cpd.JacDiagABInv;
float normalImpulse = penetrationImpulse + velocityImpulse;
// See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
float oldNormalImpulse = cpd.AppliedImpulse;
float sum = oldNormalImpulse + normalImpulse;
cpd.AppliedImpulse = 0 > sum ? 0 : sum;
normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;
if (bodyA.InverseMass != 0)
{
bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);
}
if (bodyB.InverseMass != 0)
{
bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);
}
{
//friction
Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA);
Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB);
Vector3 vel3 = vel12 - vel22;
relVel = Vector3.Dot(normal, vel3);
Vector3 latVel = vel3 - normal * relVel;
float lat_rel_vel = latVel.Length();
float combinedFriction = cpd.Friction;
if (cpd.AppliedImpulse > 0)
if (lat_rel_vel > float.Epsilon)
{
latVel /= lat_rel_vel;
Vector3 temp1 = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld);
Vector3 temp2 = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld);
float friction_impulse = lat_rel_vel /
(bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB)));
float normal_impulse = cpd.AppliedImpulse * combinedFriction;
MathHelper.SetMin(ref friction_impulse, normal_impulse);
MathHelper.SetMin(ref friction_impulse, -normal_impulse);
bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA);
bodyB.ApplyImpulse(latVel * friction_impulse, relPosB);
}
}
return normalImpulse;
}
public static float ResolveSingleFrictionEmpty(
RigidBody bodyA,
RigidBody bodyB,
ManifoldPoint contactPoint,
ContactSolverInfo solverInfo)
{
return 0;
}
}
}

62
libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs
View File

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

440
libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs
View File

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

246
libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs
View File

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

32
libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs
View File

@ -1,32 +0,0 @@
/*
Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any damages
arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software
in a product, an acknowledgment in the product documentation would be
appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
using System;
using System.Collections.Generic;
using System.Text;
namespace XnaDevRu.BulletX.Dynamics
{
public interface IConstraintSolver
{
float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo info, IDebugDraw debugDrawer);
}
}

155
libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs
View File

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

157
libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs
View File

@ -1,157 +0,0 @@
/*
Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any damages
arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software
in a product, an acknowledgment in the product documentation would be
appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
using System;
using System.Collections.Generic;
using System.Text;
using MonoXnaCompactMaths;
namespace XnaDevRu.BulletX.Dynamics
{
public class ConstraintSetting
{
private float _tau, _damping;
public ConstraintSetting()
{
_tau = 0.3f;
_damping = 1.0f;
}
public float Damping
{
get { return _damping; }
set { _damping = value; }
}
public float Tau
{
get { return _tau; }
set { _tau = value; }
}
}
public class Point2PointConstraint : TypedConstraint
{
private JacobianEntry[] _jacobian;
private Vector3 _pivotInA, _pivotInB;
private ConstraintSetting _setting = new ConstraintSetting();
public Point2PointConstraint()
{
_jacobian = new JacobianEntry[3];
}
public Point2PointConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB)
: base(rbA, rbB)
{
_jacobian = new JacobianEntry[3];
_pivotInA = pivotInA;
_pivotInB = pivotInB;
}
public Point2PointConstraint(RigidBody rbA, Vector3 pivotInA)
: base(rbA)
{
_jacobian = new JacobianEntry[3];
_pivotInA = pivotInA;
_pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, _pivotInA);
}
public ConstraintSetting Settings { get { return _setting; } set { _setting = value; } }
public Vector3 PivotInA
{
set
{
_pivotInA = value;
}
}
public Vector3 PivotInB
{
set
{
_pivotInB = value;
}
}
public override void BuildJacobian()
{
Vector3 normal = new Vector3();
for (int i = 0; i < 3; i++)
{
MathHelper.SetElement(ref normal, i, 1);
_jacobian[i] = new JacobianEntry(
MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition,
MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition,
normal,
RigidBodyA.InvInertiaDiagLocal,
RigidBodyA.InverseMass,
RigidBodyB.InvInertiaDiagLocal,
RigidBodyB.InverseMass
);
MathHelper.SetElement(ref normal, i, 0);
}
}
public override void SolveConstraint(float timeStep)
{
Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform);
Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform);
Vector3 normal = new Vector3();
for (int i = 0; i < 3; i++)
{
MathHelper.SetElement(ref normal, i, 1);
float jacDiagABInv = 1.0f / _jacobian[i].Diagonal;
Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;
Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1);
Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2);
Vector3 vel = vel1 - vel2;
float rel_vel = Vector3.Dot(normal, vel);
float depth = -Vector3.Dot((pivotAInW - pivotBInW), normal);
float impulse = depth * _setting.Tau / timeStep * jacDiagABInv - _setting.Damping * rel_vel * jacDiagABInv;
AppliedImpulse += impulse;
Vector3 impulseVector = normal * impulse;
RigidBodyA.ApplyImpulse(impulseVector, pivotAInW - RigidBodyA.CenterOfMassPosition);
RigidBodyB.ApplyImpulse(-impulseVector, pivotBInW - RigidBodyB.CenterOfMassPosition);
MathHelper.SetElement(ref normal, i, 0);
}
}
}
}

915
libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs
View File

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

188
libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs
View File

@ -1,188 +0,0 @@
/*
Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any damages
arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software
in a product, an acknowledgment in the product documentation would be
appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
using System;
using System.Collections.Generic;
using System.Text;
using MonoXnaCompactMaths;
namespace XnaDevRu.BulletX.Dynamics
{
/// <summary>
/// constraint class used for lateral tyre friction
/// </summary>
public class Solve2LinearConstraint
{
private float _tau;
private float _damping;
public Solve2LinearConstraint(float tau, float damping)
{
_tau = tau;
_damping = damping;
}
// solve unilateral constraint (equality, direct method)
public void ResolveUnilateralPairConstraint(
RigidBody body1, RigidBody body2,
Matrix world2A,
Matrix world2B,
Vector3 invInertiaADiag,
float invMassA,
Vector3 linvelA, Vector3 angvelA,
Vector3 rel_posA1,
Vector3 invInertiaBDiag,
float invMassB,
Vector3 linvelB, Vector3 angvelB,
Vector3 rel_posA2,
float depthA, Vector3 normalA,
Vector3 rel_posB1, Vector3 rel_posB2,
float depthB, Vector3 normalB,
out float imp0, out float imp1)
{
imp0 = 0;
imp1 = 0;
float len = Math.Abs(normalA.Length()) - 1f;
if (Math.Abs(len) >= float.Epsilon)
return;
BulletDebug.Assert(len < float.Epsilon);
//this jacobian entry could be re-used for all iterations
JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
invInertiaBDiag, invMassB);
JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
invInertiaBDiag, invMassB);
float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1));
float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1));
// btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
float massTerm = 1f / (invMassA + invMassB);
// calculate rhs (or error) terms
float dv0 = depthA * _tau * massTerm - vel0 * _damping;
float dv1 = depthB * _tau * massTerm - vel1 * _damping;
float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);
float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag);
imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet;
imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet;
}
// solving 2x2 lcp problem (inequality, direct solution )
public void ResolveBilateralPairConstraint(
RigidBody body1, RigidBody body2,
Matrix world2A, Matrix world2B,
Vector3 invInertiaADiag,
float invMassA,
Vector3 linvelA, Vector3 angvelA,
Vector3 rel_posA1,
Vector3 invInertiaBDiag,
float invMassB,
Vector3 linvelB, Vector3 angvelB,
Vector3 rel_posA2,
float depthA, Vector3 normalA,
Vector3 rel_posB1, Vector3 rel_posB2,
float depthB, Vector3 normalB,
out float imp0, out float imp1)
{
imp0 = 0f;
imp1 = 0f;
float len = Math.Abs(normalA.Length()) - 1f;
if (Math.Abs(len) >= float.Epsilon)
return;
BulletDebug.Assert(len < float.Epsilon);
JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
invInertiaBDiag, invMassB);
JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
invInertiaBDiag, invMassB);
float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1));
float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1));
// calculate rhs (or error) terms
float dv0 = depthA * _tau - vel0 * _damping;
float dv1 = depthB * _tau - vel1 * _damping;
float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);
float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag);
imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet;
imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet;
if (imp0 > 0.0f)
{
if (imp1 <= 0.0f)
{
imp1 = 0f;
// now imp0>0 imp1<0
imp0 = dv0 / jacA.Diagonal;
if (imp0 < 0.0f)
imp0 = 0f;
}
}
else
{
imp0 = 0f;
imp1 = dv1 / jacB.Diagonal;
if (imp1 <= 0.0f)
{
imp1 = 0f;
// now imp0>0 imp1<0
imp0 = dv0 / jacA.Diagonal;
if (imp0 > 0.0f)
{
}
else
{
imp0 = 0f;
}
}
}
}
//public void ResolveAngularConstraint(
// Matrix invInertiaAWS,
// float invMassA,
// Vector3 linvelA, Vector3 angvelA,
// Vector3 rel_posA1,
// Matrix invInertiaBWS,
// float invMassB,
// Vector3 linvelB, Vector3 angvelB,
// Vector3 rel_posA2,
// float depthA, Vector3 normalA,
// Vector3 rel_posB1, Vector3 rel_posB2,
// float depthB, Vector3 normalB,
// out float imp0, out float imp1)
//{
// imp0 = 0;
// imp1 = 0;
//}
}
}

78
libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs
View File

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

77
libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs
View File

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

88
libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs
View File

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

790
libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs
View File

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

59
libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs
View File

@ -1,59 +0,0 @@
/*
Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any damages
arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software
in a product, an acknowledgment in the product documentation would be
appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
using System;
using System.Collections.Generic;
using System.Text;
using MonoXnaCompactMaths;
namespace XnaDevRu.BulletX.Dynamics
{
public abstract class DynamicsWorld : CollisionWorld
{
public DynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache)
: base(dispatcher, pairCache) { }
//once a rigidbody is added to the dynamics world, it will get this gravity assigned
//existing rigidbodies in the world get gravity assigned too, during this method
public abstract Vector3 Gravity { set; }
public abstract IConstraintSolver ConstraintSolver { set; }
public virtual int ConstraintsCount { get { return 0; } }
public abstract IDebugDraw DebugDrawer { get; set; }
//stepSimulation proceeds the simulation over timeStep units
public abstract void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep);
public void StepSimulation(float timeStep)
{
StepSimulation(timeStep, 1, 1f / 60f);
}
public abstract void UpdateAabbs();
public virtual void AddConstraint(TypedConstraint constraint) { }
public virtual void RemoveConstraint(TypedConstraint constraint) { }
public abstract void AddRigidBody(RigidBody body);
public abstract void RemoveRigidBody(RigidBody body);
public virtual TypedConstraint GetConstraint(int index) { return null; }
}
}

Some files were not shown because too many files have changed in this diff Show More