aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/libraries
diff options
context:
space:
mode:
Diffstat (limited to 'libraries')
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX.sln42
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX.suobin0 -> 138240 bytes
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs68
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/BulletX.snkbin0 -> 160 bytes
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs623
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs68
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs113
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs91
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs51
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs42
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs54
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs36
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs42
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs33
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs159
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs128
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs46
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs59
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs40
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs280
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs163
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs48
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs358
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs157
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs189
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs193
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs130
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs52
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs147
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs304
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs270
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs104
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs100
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs214
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs151
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs215
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs316
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs83
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs148
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs183
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs55
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs208
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs184
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs141
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs185
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs136
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs100
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs100
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs80
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs55
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs33
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs58
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs99
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs154
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs32
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs293
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs63
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs133
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs116
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs124
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs115
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs67
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs80
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs33
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs136
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs102
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs160
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs187
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs199
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs73
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs117
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs176
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs633
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs56
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs101
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs343
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs42
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs48
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs78
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs246
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs272
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs64
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs142
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs115
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs643
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs488
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs62
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs440
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs246
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs32
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs155
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs157
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs915
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs188
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs78
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs77
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs88
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs790
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs59
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs447
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs211
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs94
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/VehicleRaycaster.cs49
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs529
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs53
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs60
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs66
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs581
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs116
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs44
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs88
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs198
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs102
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs221
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs110
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj168
-rw-r--r--libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs35
-rw-r--r--libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs683
-rw-r--r--libraries/ModifiedBulletX/MonoXnaCompactMaths/MonoXnaCompactMaths.csproj51
-rw-r--r--libraries/ModifiedBulletX/MonoXnaCompactMaths/Properties/AssemblyInfo.cs33
-rw-r--r--libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs346
-rw-r--r--libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector3.cs620
-rw-r--r--libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector4.cs630
123 files changed, 21387 insertions, 0 deletions
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX.sln b/libraries/ModifiedBulletX/ModifiedBulletX.sln
new file mode 100644
index 0000000..0eccfdd
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX.sln
@@ -0,0 +1,42 @@
1
2Microsoft Visual Studio Solution File, Format Version 9.00
3# Visual C# Express 2005
4Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "MonoXnaCompactMaths", "MonoXnaCompactMaths\MonoXnaCompactMaths.csproj", "{121147BC-B06B-406C-84E9-907F268CF0EB}"
5EndProject
6Project("{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC}") = "Modified.XnaDevRu.BulletX", "ModifiedBulletX\Modified.XnaDevRu.BulletX.csproj", "{44270344-ACA7-4875-B585-81D5C06D0489}"
7EndProject
8Global
9 GlobalSection(SolutionConfigurationPlatforms) = preSolution
10 Debug|Any CPU = Debug|Any CPU
11 Debug|Mixed Platforms = Debug|Mixed Platforms
12 Debug|x86 = Debug|x86
13 Release|Any CPU = Release|Any CPU
14 Release|Mixed Platforms = Release|Mixed Platforms
15 Release|x86 = Release|x86
16 EndGlobalSection
17 GlobalSection(ProjectConfigurationPlatforms) = postSolution
18 {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
19 {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Any CPU.Build.0 = Debug|Any CPU
20 {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Mixed Platforms.ActiveCfg = Debug|Any CPU
21 {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|Mixed Platforms.Build.0 = Debug|Any CPU
22 {121147BC-B06B-406C-84E9-907F268CF0EB}.Debug|x86.ActiveCfg = Debug|Any CPU
23 {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Any CPU.ActiveCfg = Release|Any CPU
24 {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Any CPU.Build.0 = Release|Any CPU
25 {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Mixed Platforms.ActiveCfg = Release|Any CPU
26 {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|Mixed Platforms.Build.0 = Release|Any CPU
27 {121147BC-B06B-406C-84E9-907F268CF0EB}.Release|x86.ActiveCfg = Release|Any CPU
28 {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Any CPU.ActiveCfg = Debug|Any CPU
29 {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Any CPU.Build.0 = Debug|Any CPU
30 {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Mixed Platforms.ActiveCfg = Debug|Any CPU
31 {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|Mixed Platforms.Build.0 = Debug|Any CPU
32 {44270344-ACA7-4875-B585-81D5C06D0489}.Debug|x86.ActiveCfg = Debug|Any CPU
33 {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Any CPU.ActiveCfg = Release|Any CPU
34 {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Any CPU.Build.0 = Release|Any CPU
35 {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Mixed Platforms.ActiveCfg = Release|Any CPU
36 {44270344-ACA7-4875-B585-81D5C06D0489}.Release|Mixed Platforms.Build.0 = Release|Any CPU
37 {44270344-ACA7-4875-B585-81D5C06D0489}.Release|x86.ActiveCfg = Release|Any CPU
38 EndGlobalSection
39 GlobalSection(SolutionProperties) = preSolution
40 HideSolutionNode = FALSE
41 EndGlobalSection
42EndGlobal
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX.suo b/libraries/ModifiedBulletX/ModifiedBulletX.suo
new file mode 100644
index 0000000..4f0acd9
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX.suo
Binary files differ
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs b/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs
new file mode 100644
index 0000000..d0d5845
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/BulletDebug.cs
@@ -0,0 +1,68 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using System.Diagnostics;
26
27namespace XnaDevRu.BulletX
28{
29 internal static class BulletDebug
30 {
31 [Conditional("DEBUG")]
32 public static void Assert(Boolean condition)
33 {
34 //if (!condition)
35 //{
36 // Throw("No info available");
37 //}
38 Debug.Assert(condition);
39 }
40
41 [Conditional("DEBUG")]
42 public static void Assert(Boolean condition, String message)
43 {
44 //if (!condition)
45 //{
46 // Throw(message);
47 //}
48 Debug.Assert(condition, message);
49 }
50
51 [Conditional("DEBUG")]
52 public static void Assert(Boolean condition, String message, String detailMessage)
53 {
54 //if (!condition)
55 //{
56 // Throw(message);
57 //}
58 Debug.Assert(condition, message, detailMessage);
59 }
60
61 private static void Throw(String message)
62 {
63 String msg = String.Format("Assertion Error: {0}", message);
64
65 throw new BulletException(msg);
66 }
67 }
68}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/BulletX.snk b/libraries/ModifiedBulletX/ModifiedBulletX/BulletX.snk
new file mode 100644
index 0000000..4ce5907
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/BulletX.snk
Binary files differ
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs
new file mode 100644
index 0000000..12692ea
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/AxisSweep3.cs
@@ -0,0 +1,623 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class AxisSweep3: OverlappingPairCache
30 {
31 Vector3 _worldAabbMin;
32 Vector3 _worldAabbMax;
33
34 Vector3 _quantize;
35
36 int _numHandles;
37 int _maxHandles;
38
39 Handle[] _handles;
40 Edge[][] _edges = new Edge[3][];
41
42 ushort _firstFreeHandle;
43
44 int _invalidPair;
45
46 public AxisSweep3(Vector3 worldAabbMin, Vector3 worldAabbMax, int maxHandles)
47 : base()
48 {
49 BulletDebug.Assert(maxHandles > 1 && maxHandles < 32767);
50
51 // init bounds
52 _worldAabbMin = worldAabbMin;
53 _worldAabbMax = worldAabbMax;
54
55 Vector3 aabbSize = _worldAabbMax - _worldAabbMin;
56 _quantize = new Vector3(65535.0f, 65535.0f, 65535.0f) / aabbSize;
57
58 // allocate handles buffer and put all handles on free list
59 _handles = new Handle[maxHandles];
60 for (int i = 0; i < maxHandles; i++)
61 _handles[i] = new Handle();
62 _maxHandles = maxHandles;
63 _numHandles = 0;
64
65 // handle 0 is reserved as the null index, and is also used as the sentinel
66 _firstFreeHandle = 1;
67 {
68 for (int i = _firstFreeHandle; i < maxHandles; i++)
69 {
70 _handles[i].NextFree = (ushort)(i + 1);
71 }
72 _handles[maxHandles - 1].NextFree = 0;
73 }
74
75 {
76 // allocate edge buffers
77 for (int i = 0; i < 3; i++)
78 {
79 _edges[i] = new Edge[maxHandles * 2];
80 for (int j = 0; j < maxHandles * 2; j++)
81 {
82 _edges[i][j] = new Edge();
83 }
84 }
85 }
86 //removed overlap management
87
88 // make boundary sentinels
89
90 _handles[0].ClientData = 0;
91
92 for (int axis = 0; axis < 3; axis++)
93 {
94 _handles[0].MinEdges[axis] = 0;
95 _handles[0].MaxEdges[axis] = 1;
96
97 _edges[axis][0].Position = 0;
98 _edges[axis][0].Handle = 0;
99 _edges[axis][1].Position = 0xffff;
100 _edges[axis][1].Handle = 0;
101 }
102 }
103
104 public ushort AddHandle(Vector3 aabbMin, Vector3 aabbMax, object owner, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask)
105 {
106 ushort[] min = new ushort[3], max = new ushort[3];
107 Quantize(out min, aabbMin, 0);
108 Quantize(out max, aabbMax, 1);
109
110 ushort handle = AllocateHandle();
111 Handle oHandle = GetHandle(handle);
112
113 oHandle.HandleID = handle;
114 oHandle.ClientData = owner;
115 oHandle.CollisionFilterGroup = collisionFilterGroup;
116 oHandle.CollisionFilterMask = collisionFilterMask;
117
118 int limit = _numHandles * 2;
119
120 // (Gluk )
121 // ( Inside )
122 for (int axis = 0; axis < 3; axis++)
123 {
124 _handles[0].MaxEdges[axis] += 2;
125
126 _edges[axis][limit + 1].Position = _edges[axis][limit - 1].Position;
127 _edges[axis][limit + 1].Handle = _edges[axis][limit - 1].Handle;
128
129 _edges[axis][limit - 1].Position = min[axis];
130 _edges[axis][limit - 1].Handle = handle;
131
132 _edges[axis][limit].Position = max[axis];
133 _edges[axis][limit].Handle = handle;
134
135 oHandle.MinEdges[axis] = (ushort)(limit - 1);
136 oHandle.MaxEdges[axis] = (ushort)limit;
137 }
138
139 SortMinDown(0, oHandle.MinEdges[0], false);
140 SortMaxDown(0, oHandle.MaxEdges[0], false);
141 SortMinDown(1, oHandle.MinEdges[1], false);
142 SortMaxDown(1, oHandle.MaxEdges[1], false);
143 SortMinDown(2, oHandle.MinEdges[2], true);
144 SortMaxDown(2, oHandle.MaxEdges[2], true);
145
146 return handle;
147 }
148
149 public void RemoveHandle(ushort handle)
150 {
151 Handle pHandle = GetHandle(handle);
152
153 //explicitly remove the pairs containing the proxy
154 //we could do it also in the sortMinUp (passing true)
155 //todo: compare performance
156 RemoveOverlappingPairsContainingProxy(pHandle);
157
158
159 // compute current limit of edge arrays
160 int limit = _numHandles * 2;
161 int axis;
162
163 for (axis = 0; axis < 3; axis++)
164 {
165 _handles[0].MaxEdges[axis] -= 2;
166 }
167
168 // remove the edges by sorting them up to the end of the list
169 for (axis = 0; axis < 3; axis++)
170 {
171 Edge[] pEdges = _edges[axis];
172 ushort max = pHandle.MaxEdges[axis];
173 pEdges[max].Position = 0xffff;
174
175 SortMaxUp(axis, max, false);
176
177 ushort i = pHandle.MinEdges[axis];
178 pEdges[i].Position = 0xffff;
179
180 SortMinUp(axis, i, false);
181
182 pEdges[limit - 1].Handle = 0;
183 pEdges[limit - 1].Position = 0xffff;
184 }
185
186 // free the handle
187 FreeHandle(handle);
188 }
189
190 public override void ProcessAllOverlappingPairs(IOverlapCallback callback)
191 {
192 OverlappingPairs.Sort(new Comparison<BroadphasePair>(BroadphasePair.ComparisonSort));
193
194 if (_invalidPair != 0)
195 OverlappingPairs.RemoveRange(OverlappingPairs.Count - _invalidPair, _invalidPair);
196 _invalidPair = 0;
197
198 BroadphasePair previousPair = new BroadphasePair();
199 previousPair.ProxyA = null;
200 previousPair.ProxyB = null;
201 previousPair.CollisionAlgorithm = null;
202
203 List<BroadphasePair> removal = new List<BroadphasePair>();
204
205 for (int i = 0; i < OverlappingPairs.Count; i++)
206 {
207 bool isDuplicate = (OverlappingPairs[i] == previousPair);
208 previousPair = OverlappingPairs[i];
209 bool needsRemoval;
210 if (!isDuplicate)
211 {
212 bool hasOverlap = TestOverlap(previousPair.ProxyA, previousPair.ProxyB);
213 if (hasOverlap)
214 {
215 needsRemoval = callback.ProcessOverlap(ref previousPair);
216 }
217 else
218 {
219 needsRemoval = true;
220 }
221 }
222 else
223 {
224 needsRemoval = true;
225 BulletDebug.Assert(previousPair.CollisionAlgorithm == null);
226 }
227
228 if (needsRemoval)
229 {
230 removal.Add(previousPair);
231 }
232 }
233
234 for (int i = 0; i < removal.Count; i++)
235 {
236 BroadphasePair pair = removal[i];
237 CleanOverlappingPair(ref pair);
238 pair.ProxyA = null;
239 pair.ProxyB = null;
240 _invalidPair++;
241 OverlappingPairCount--;
242 }
243 }
244
245 private bool TestOverlap(BroadphaseProxy proxyA, BroadphaseProxy proxyB)
246 {
247 if (proxyA == null || proxyB == null)
248 return false;
249
250 Handle handleA = proxyA as Handle;
251 Handle handleB = proxyB as Handle;
252
253 for (int axis = 0; axis < 3; axis++)
254 {
255 if (handleA.MaxEdges[axis] < handleB.MinEdges[axis] ||
256 handleB.MaxEdges[axis] < handleA.MinEdges[axis])
257 {
258 return false;
259 }
260 }
261 return true;
262 }
263
264 private bool TestOverlap(int ignoreAxis, Handle pHandleA, Handle pHandleB)
265 {
266 for (int axis = 0; axis < 3; axis++)
267 {
268 if (axis != ignoreAxis)
269 {
270 if (pHandleA.MaxEdges[axis] < pHandleB.MinEdges[axis] ||
271 pHandleB.MaxEdges[axis] < pHandleA.MinEdges[axis])
272 {
273 return false;
274 }
275 }
276 }
277
278 return true;
279 }
280
281 private ushort AllocateHandle()
282 {
283 ushort handle = _firstFreeHandle;
284 _firstFreeHandle = GetHandle(handle).NextFree;
285 _numHandles++;
286
287 return handle;
288 }
289
290 private void FreeHandle(ushort handle)
291 {
292 BulletDebug.Assert(handle > 0 && handle < _maxHandles);
293
294 GetHandle(handle).NextFree = _firstFreeHandle;
295 _firstFreeHandle = handle;
296
297 _numHandles--;
298 }
299
300 private Handle GetHandle(ushort handle)
301 {
302 return _handles[handle];
303 }
304
305 private void UpdateHandle(ushort handle, Vector3 aabbMin, Vector3 aabbMax)
306 {
307 Handle pHandle = GetHandle(handle);
308
309 // quantize the new bounds
310 ushort[] min = new ushort[3];
311 ushort[] max = new ushort[3];
312 Quantize(out min, aabbMin, 0);
313 Quantize(out max, aabbMax, 1);
314
315 // update changed edges
316 for (int axis = 0; axis < 3; axis++)
317 {
318 ushort emin = pHandle.MinEdges[axis];
319 ushort emax = pHandle.MaxEdges[axis];
320
321 int dmin = (int)min[axis] - (int)_edges[axis][emin].Position;
322 int dmax = (int)max[axis] - (int)_edges[axis][emax].Position;
323
324 _edges[axis][emin].Position = min[axis];
325 _edges[axis][emax].Position = max[axis];
326
327 // expand (only adds overlaps)
328 if (dmin < 0)
329 SortMinDown(axis, emin, true);
330
331 if (dmax > 0)
332 SortMaxUp(axis, emax, true);
333
334 // shrink (only removes overlaps)
335 if (dmin > 0)
336 SortMinUp(axis, emin, true);
337
338 if (dmax < 0)
339 SortMaxDown(axis, emax, true);
340 }
341 }
342
343 private void Quantize(out ushort[] result, Vector3 point, int isMax)
344 {
345 Vector3 clampedPoint = new Vector3(
346 point.X,
347 point.Y,
348 point.Z
349 );
350
351 MathHelper.SetMax(ref clampedPoint, _worldAabbMin);
352 MathHelper.SetMin(ref clampedPoint, _worldAabbMax);
353
354 Vector3 v = (clampedPoint - _worldAabbMin) * _quantize;
355
356 result = new ushort[3];
357 result[0] = (ushort)(((int)v.X & 0xfffe) | isMax);
358 result[1] = (ushort)(((int)v.Y & 0xfffe) | isMax);
359 result[2] = (ushort)(((int)v.Z & 0xfffe) | isMax);
360 }
361
362 private void SortMinDown(int axis, ushort edge, bool updateOverlaps)
363 {
364 Edge pEdge = _edges[axis][edge];
365 Edge pPrev = _edges[axis][edge - 1];
366 Handle pHandleEdge = GetHandle(pEdge.Handle);
367
368 while (pEdge.Position < pPrev.Position)
369 {
370 Handle pHandlePrev = GetHandle(pPrev.Handle);
371
372 if (pPrev.IsMax())
373 {
374 // if previous edge is a maximum check the bounds and add an overlap if necessary
375 if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandlePrev))
376 {
377 AddOverlappingPair(pHandleEdge, pHandlePrev);
378 }
379
380 // update edge reference in other handle
381 pHandlePrev.MaxEdges[axis]++;
382 }
383 else
384 pHandlePrev.MinEdges[axis]++;
385
386 pHandleEdge.MinEdges[axis]--;
387
388 // swap the edges
389 pEdge.Swap(ref pPrev);
390
391 // decrement
392 edge--;
393 pEdge = _edges[axis][edge];
394 pPrev = _edges[axis][edge - 1];
395 }
396 }
397
398 private void SortMinUp(int axis, ushort edge, bool updateOverlaps)
399 {
400 Edge pEdge = _edges[axis][edge];
401 Edge pNext = _edges[axis][edge + 1];
402 Handle pHandleEdge = GetHandle(pEdge.Handle);
403
404 while ((pNext.Handle != 0) && (pEdge.Position >= pNext.Position))
405 {
406 Handle pHandleNext = GetHandle(pNext.Handle);
407
408 if (pNext.IsMax())
409 {
410 // if next edge is maximum remove any overlap between the two handles
411 if (updateOverlaps)
412 {
413 //Handle handle0 = GetHandle(pEdge.Handle);
414 //Handle handle1 = GetHandle(pNext.Handle);
415 //BroadphasePair tmpPair = new BroadphasePair(handle0, handle1);
416 //RemoveOverlappingPair(tmpPair);
417 }
418
419 // update edge reference in other handle
420 pHandleNext.MaxEdges[axis]--;
421 }
422 else
423 pHandleNext.MinEdges[axis]--;
424
425 pHandleEdge.MinEdges[axis]++;
426
427 // swap the edges
428 pEdge.Swap(ref pNext);
429
430 // increment
431 edge++;
432 pEdge = _edges[axis][edge];
433 pNext = _edges[axis][edge + 1];
434 }
435 }
436
437 private void SortMaxDown(int axis, ushort edge, bool updateOverlaps)
438 {
439 Edge pEdge = _edges[axis][edge];
440 Edge pPrev = _edges[axis][edge - 1];
441 Handle pHandleEdge = GetHandle(pEdge.Handle);
442
443 while (pEdge.Position < pPrev.Position)
444 {
445 Handle pHandlePrev = GetHandle(pPrev.Handle);
446
447 if (!pPrev.IsMax())
448 {
449 // if previous edge was a minimum remove any overlap between the two handles
450 if (updateOverlaps)
451 {
452 //this is done during the overlappingpairarray iteration/narrowphase collision
453 //Handle handle0 = GetHandle(pEdge.Handle);
454 //Handle handle1 = GetHandle(pPrev.Handle);
455 //BroadphasePair pair = FindPair(handle0, handle1);
456
457 //if (pair != null)
458 //{
459 // RemoveOverlappingPair(pair);
460 //}
461 }
462
463 // update edge reference in other handle
464 pHandlePrev.MinEdges[axis]++; ;
465 }
466 else
467 pHandlePrev.MaxEdges[axis]++;
468
469 pHandleEdge.MaxEdges[axis]--;
470
471 // swap the edges
472 pEdge.Swap(ref pPrev);
473
474 // decrement
475 edge--;
476 pEdge = _edges[axis][edge];
477 pPrev = _edges[axis][edge - 1];
478 }
479 }
480
481 private void SortMaxUp(int axis, ushort edge, bool updateOverlaps)
482 {
483 Edge pEdge = _edges[axis][edge];
484 Edge pNext = _edges[axis][edge + 1];
485 Handle pHandleEdge = GetHandle(pEdge.Handle);
486
487 while ((pNext.Handle!=0) && (pEdge.Position >= pNext.Position))
488 {
489 Handle pHandleNext = GetHandle(pNext.Handle);
490
491 if (!pNext.IsMax())
492 {
493 // if next edge is a minimum check the bounds and add an overlap if necessary
494 if (updateOverlaps && TestOverlap(axis, pHandleEdge, pHandleNext))
495 {
496 Handle handle0 = GetHandle(pEdge.Handle);
497 Handle handle1 = GetHandle(pNext.Handle);
498 AddOverlappingPair(handle0, handle1);
499 }
500
501 // update edge reference in other handle
502 pHandleNext.MinEdges[axis]--;
503 }
504 else
505 pHandleNext.MaxEdges[axis]--;
506
507 pHandleEdge.MaxEdges[axis]++;
508
509 // swap the edges
510 pEdge.Swap(ref pNext);
511
512 // increment
513 edge++;
514 pEdge = _edges[axis][edge];
515 pNext = _edges[axis][edge + 1];
516 }
517 }
518
519 #region Abstract
520
521 public override void RefreshOverlappingPairs()
522 {
523 }
524
525 public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask)
526 {
527 ushort handleId = AddHandle(min, max, userData, collisionFilterGroup, collisionFilterMask);
528
529 Handle handle = GetHandle(handleId);
530
531 return handle;
532 }
533
534 public override void DestroyProxy(BroadphaseProxy proxy)
535 {
536 Handle handle = proxy as Handle;
537 RemoveHandle(handle.HandleID);
538 }
539
540 public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax)
541 {
542 Handle handle = proxy as Handle;
543 UpdateHandle(handle.HandleID, aabbMin, aabbMax);
544 }
545 #endregion
546 }
547
548 public class Edge
549 {
550 ushort position;
551 ushort handle;
552
553 public ushort Position
554 {
555 get { return position; }
556 set { position = value; }
557 }
558
559 public ushort Handle
560 {
561 get { return handle; }
562 set { handle = value; }
563 }
564
565 public bool IsMax()
566 {
567 return (position & (ushort)1) == 1;
568 }
569
570 public void Swap(ref Edge e)
571 {
572 ushort tmpPosition = this.position;
573 ushort tmpHandle = this.handle;
574 this.position = e.position;
575 this.handle = e.handle;
576 e.position = tmpPosition;
577 e.handle = tmpHandle;
578 }
579 }
580
581 public class Handle: BroadphaseProxy
582 {
583 ushort[] minEdges, maxEdges;
584 ushort pad;
585 ushort handleID;
586
587 public ushort[] MinEdges
588 {
589 get { return minEdges; }
590 set { minEdges = value; }
591 }
592
593 public ushort[] MaxEdges
594 {
595 get { return maxEdges; }
596 set { maxEdges = value; }
597 }
598
599 public ushort HandleID
600 {
601 get { return handleID; }
602 set { handleID = value; }
603 }
604
605 public ushort Pad
606 {
607 get { return pad; }
608 set { pad = value; }
609 }
610
611 public ushort NextFree
612 {
613 get { return minEdges[0]; }
614 set { minEdges[0] = value;}
615 }
616
617 public Handle()
618 {
619 minEdges = new ushort[3];
620 maxEdges = new ushort[3];
621 }
622 }
623}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs
new file mode 100644
index 0000000..dcfd0d8
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseNativeTypes.cs
@@ -0,0 +1,68 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26namespace XnaDevRu.BulletX
27{
28 /// Dispatcher uses these types
29 /// IMPORTANT NOTE:The types are ordered polyhedral, implicit convex and concave
30 /// to facilitate type checking
31 public enum BroadphaseNativeTypes
32 {
33 // polyhedral convex shapes
34 Box,
35 Triangle,
36 Tetrahedral,
37 ConvexTriangleMesh,
38 ConvexHull,
39 //implicit convex shapes
40 ImplicitConvexShapes,
41 Sphere,
42 MultiSphere,
43 Capsule,
44 Cone,
45 Convex,
46 Cylinder,
47 MinkowskiSum,
48 MinkowskiDifference,
49 //concave shapes
50 ConcaveShapesStart,
51 //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy!
52 TriangleMesh,
53 //used for demo integration FAST/Swift collision library and Bullet
54 FastConcaveMesh,
55 //terrain
56 Terrain,
57 //Used for GIMPACT Trimesh integration
58 Gimpact,
59
60 Empty,
61 StaticPlane,
62 ConcaveShapesEnd,
63
64 Compound,
65
66 MaxBroadphaseCollisionTypes,
67 }
68}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs
new file mode 100644
index 0000000..2b9e114
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphasePair.cs
@@ -0,0 +1,113 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26namespace XnaDevRu.BulletX
27{
28 public class BroadphasePair
29 {
30 private BroadphaseProxy _proxyA;
31 private BroadphaseProxy _proxyB;
32
33 private CollisionAlgorithm _algorithm;
34 private object _userInfo;
35
36 public BroadphasePair()
37 {
38 }
39
40 public BroadphasePair(BroadphasePair other)
41 {
42 _proxyA = other._proxyA;
43 _proxyB = other._proxyB;
44
45 _algorithm = other._algorithm;
46 _userInfo = null;
47 }
48
49 public BroadphasePair(BroadphaseProxy proxyA, BroadphaseProxy proxyB)
50 {
51 _proxyA = proxyA;
52 _proxyB = proxyB;
53
54 _algorithm = null;
55 _userInfo = null;
56 }
57
58 public BroadphaseProxy ProxyA { get { return _proxyA; } set { _proxyA = value; } }
59 public BroadphaseProxy ProxyB { get { return _proxyB; } set { _proxyB = value; } }
60
61 public CollisionAlgorithm CollisionAlgorithm { get { return _algorithm; } set { _algorithm = value; } }
62 public object UserInfo { get { return _userInfo; } set { _userInfo = value; } }
63
64 public override int GetHashCode()
65 {
66 return _proxyA.GetHashCode() ^ _proxyB.GetHashCode();
67 }
68
69 public override bool Equals(object obj)
70 {
71 if (obj is BroadphasePair)
72 return this == (BroadphasePair)obj;
73 return false;
74 }
75
76 public static int ComparisonSort(BroadphasePair a, BroadphasePair b)
77 {
78 int aAId = a.ProxyA != null ? a.ProxyA.ComparisonID : -1;
79 int aBId = a.ProxyB != null ? a.ProxyB.ComparisonID : -1;
80 int aCId = a.CollisionAlgorithm != null ? a.CollisionAlgorithm.ComparisonID : -1;
81 int bAId = b.ProxyA != null ? b.ProxyA.ComparisonID : -1;
82 int bBId = b.ProxyB != null ? b.ProxyB.ComparisonID : -1;
83 int bCId = b.CollisionAlgorithm != null ? b.CollisionAlgorithm.ComparisonID : -1;
84
85 if (aAId > bAId ||
86 (a.ProxyA == b.ProxyA && aBId > bBId) ||
87 (a.ProxyA == b.ProxyA && a.ProxyB == b.ProxyB && aCId > bCId))
88 return -1;
89 else
90 return 1;
91 }
92
93 public static bool operator ==(BroadphasePair a, BroadphasePair b)
94 {
95 if (object.Equals(a, null) && object.Equals(b, null))
96 return true;
97 if (object.Equals(a, null) || object.Equals(b, null))
98 return false;
99
100 return (a.ProxyA == b.ProxyA) && (a.ProxyB == b.ProxyB);
101 }
102
103 public static bool operator !=(BroadphasePair a, BroadphasePair b)
104 {
105 if (object.Equals(a, null) && object.Equals(b, null))
106 return true;
107 if (object.Equals(a, null) || object.Equals(b, null))
108 return false;
109
110 return (a.ProxyA != b.ProxyA) || (a.ProxyB != b.ProxyB);
111 }
112 }
113}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs
new file mode 100644
index 0000000..aed82d5
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/BroadphaseProxy.cs
@@ -0,0 +1,91 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26namespace XnaDevRu.BulletX
27{
28 public class BroadphaseProxy
29 {
30 //Usually the client CollisionObject or Rigidbody class
31 private object _clientObject;
32 private CollisionFilterGroups _collisionFilterGroup;
33 private CollisionFilterGroups _collisionFilterMask;
34 private readonly int _comparisonID;
35
36 private static int _globalCount = 0;
37
38 public BroadphaseProxy()
39 {
40 _comparisonID = _globalCount++;
41 }
42
43 public BroadphaseProxy(object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)
44 : this()
45 {
46 _clientObject = userData;
47 _collisionFilterGroup = collisionFilterGroup;
48 _collisionFilterMask = collisionFilterMask;
49 }
50
51 public object ClientData { get { return _clientObject; } set { _clientObject = value; } }
52 public CollisionFilterGroups CollisionFilterGroup { get { return _collisionFilterGroup; } set { _collisionFilterGroup = value; } }
53 public CollisionFilterGroups CollisionFilterMask { get { return _collisionFilterMask; } set { _collisionFilterMask = value; } }
54 internal int ComparisonID { get { return _comparisonID; } }
55
56 public static bool IsPolyhedral(BroadphaseNativeTypes proxyType)
57 {
58 return (proxyType < BroadphaseNativeTypes.ImplicitConvexShapes);
59 }
60
61 public static bool IsConvex(BroadphaseNativeTypes proxyType)
62 {
63 return (proxyType < BroadphaseNativeTypes.ConcaveShapesStart);
64 }
65
66 public static bool IsConcave(BroadphaseNativeTypes proxyType)
67 {
68 return ((proxyType > BroadphaseNativeTypes.ConcaveShapesStart) &&
69 (proxyType < BroadphaseNativeTypes.ConcaveShapesEnd));
70 }
71 public static bool IsCompound(BroadphaseNativeTypes proxyType)
72 {
73 return (proxyType == BroadphaseNativeTypes.Compound);
74 }
75 public static bool IsInfinite(BroadphaseNativeTypes proxyType)
76 {
77 return (proxyType == BroadphaseNativeTypes.StaticPlane);
78 }
79
80 //optional filtering to cull potential collisions
81 public enum CollisionFilterGroups
82 {
83 Default = 1,
84 Static = 2,
85 Kinematic = 4,
86 Debris = 8,
87 Sensor = 16,
88 All = Default | Static | Kinematic | Debris | Sensor,
89 }
90 }
91}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs
new file mode 100644
index 0000000..1e59ad8
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithm.cs
@@ -0,0 +1,51 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26namespace XnaDevRu.BulletX
27{
28 /// <summary>
29 /// CollisionAlgorithm is an collision interface that is compatible with the Broadphase and Dispatcher.
30 /// It is persistent over frames
31 /// </summary>
32 public abstract class CollisionAlgorithm
33 {
34 private IDispatcher _dispatcher;
35 private readonly int _comparisonID = 0;
36
37 private static int _globalCount = 0;
38
39 public CollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo)
40 {
41 _comparisonID = _globalCount++;
42 _dispatcher = collisionAlgorithmConstructionInfo.Dispatcher;
43 }
44
45 protected IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } }
46 internal int ComparisonID { get { return _comparisonID; } }
47
48 public abstract void ProcessCollision(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut);
49 public abstract float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut);
50 }
51}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs
new file mode 100644
index 0000000..381752a
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/CollisionAlgorithmConstructionInfo.cs
@@ -0,0 +1,42 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26namespace XnaDevRu.BulletX
27{
28 public struct CollisionAlgorithmConstructionInfo
29 {
30 private IDispatcher _dispatcher;
31 private PersistentManifold _manifold;
32
33 public CollisionAlgorithmConstructionInfo(IDispatcher dispatcher)
34 {
35 _dispatcher = dispatcher;
36 _manifold = null;
37 }
38
39 public IDispatcher Dispatcher { get { return _dispatcher; } set { _dispatcher = value; } }
40 public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } }
41 }
42}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs
new file mode 100644
index 0000000..8c0367f
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/DispatcherInfo.cs
@@ -0,0 +1,54 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26namespace XnaDevRu.BulletX
27{
28 public enum DispatchFunction
29 {
30 Discrete = 1,
31 Continuous,
32 }
33
34 public class DispatcherInfo
35 {
36 private float _timeStep;
37 private int _stepCount;
38 private DispatchFunction _dispatchFunc = DispatchFunction.Discrete;
39 private float _timeOfImpact = 1;
40 private bool _useContinuous;
41 private bool _enableSatConvex;
42 private bool _enableSpu;
43 private IDebugDraw _debugDraw;
44
45 public float TimeStep { get { return _timeStep; } set { _timeStep = value; } }
46 public int StepCount { get { return _stepCount; } set { _stepCount = value; } }
47 public DispatchFunction DispatchFunction { get { return _dispatchFunc; } set { _dispatchFunc = value; } }
48 public float TimeOfImpact { get { return _timeOfImpact; } set { _timeOfImpact = value; } }
49 public bool UseContinuous { get { return _useContinuous; } set { _useContinuous = value; } }
50 public bool EnableSatConvex { get { return _enableSatConvex; } set { _enableSatConvex = value; } }
51 public bool enableSpu { get { return _enableSpu; } set { _enableSpu = value; } }
52 public IDebugDraw DebugDraw { get { return _debugDraw; } set { _debugDraw = value; } }
53 }
54}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs
new file mode 100644
index 0000000..89618af
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IBroadphase.cs
@@ -0,0 +1,36 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public interface IBroadphase
30 {
31 BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask);
32 void DestroyProxy(BroadphaseProxy proxy);
33 void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax);
34 void CleanProxyFromPairs(BroadphaseProxy proxy);
35 }
36}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs
new file mode 100644
index 0000000..52831b7
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IDispatcher.cs
@@ -0,0 +1,42 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26namespace XnaDevRu.BulletX
27{
28 public interface IDispatcher
29 {
30 CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold);
31 CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB);
32 PersistentManifold GetNewManifold(object bodyA, object bodyB);
33 void ReleaseManifold(PersistentManifold manifold);
34 void ClearManifold(PersistentManifold manifold);
35 bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB);
36 bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB);
37 void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo);
38 PersistentManifold GetManifoldByIndex(int index);
39
40 int ManifoldCount { get; }
41 }
42}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs
new file mode 100644
index 0000000..4e128da
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/IOverlapCallback.cs
@@ -0,0 +1,33 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26namespace XnaDevRu.BulletX
27{
28 public interface IOverlapCallback
29 {
30 //return true for deletion of the pair
31 bool ProcessOverlap(ref BroadphasePair pair);
32 }
33}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs
new file mode 100644
index 0000000..165912a
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/OverlappingPairCache.cs
@@ -0,0 +1,159 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26namespace XnaDevRu.BulletX
27{
28 public abstract class OverlappingPairCache : IBroadphase
29 {
30 private static int _overlappingPairCount = 0;
31 private List<BroadphasePair> _overlappingPairs = new List<BroadphasePair>();
32 //during the dispatch, check that user doesn't destroy/create proxy
33 private bool _blockedForChanges;
34
35 public List<BroadphasePair> OverlappingPairs { get { return _overlappingPairs; } set { _overlappingPairs = value; } }
36 public bool BlockedForChanges { get { return _blockedForChanges; } set { _blockedForChanges = value; } }
37
38 public static int OverlappingPairCount { get { return _overlappingPairCount; } set { _overlappingPairCount = value; } }
39
40 public void RemoveOverlappingPair(BroadphasePair pair)
41 {
42 if (!_overlappingPairs.Contains(pair))
43 return;
44
45 CleanOverlappingPair(ref pair);
46 _overlappingPairs.Remove(pair);
47 }
48
49 public void AddOverlappingPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB)
50 {
51 //don't add overlap with own
52 bool test = proxyA != proxyB;
53 BulletDebug.Assert(proxyA != proxyB);
54
55 if (!NeedsBroadphaseCollision(proxyA, proxyB))
56 return;
57
58 BroadphasePair pair = new BroadphasePair(proxyA, proxyB);
59 _overlappingPairs.Add(pair);
60 _overlappingPairCount++;
61 }
62
63 //this FindPair becomes really slow. Either sort the list to speedup the query, or
64 //use a different solution. It is mainly used for Removing overlapping pairs. Removal could be delayed.
65 //we could keep a linked list in each proxy, and store pair in one of the proxies (with lowest memory address)
66 //Also we can use a 2D bitmap, which can be useful for a future GPU implementation
67 public BroadphasePair FindPair(BroadphaseProxy proxyA, BroadphaseProxy proxyB)
68 {
69 if (!NeedsBroadphaseCollision(proxyA, proxyB))
70 return null;
71
72 BroadphasePair pair = new BroadphasePair(proxyA, proxyB);
73 for (int i = 0; i < _overlappingPairs.Count; i++)
74 {
75 if (_overlappingPairs[i] == pair)
76 {
77 return _overlappingPairs[i];
78 }
79 }
80
81 return null;
82 }
83
84 public void CleanProxyFromPairs(BroadphaseProxy proxy)
85 {
86 for (int i = 0; i < _overlappingPairs.Count; i++)
87 {
88 BroadphasePair pair = _overlappingPairs[i];
89 if (pair.ProxyA == proxy ||
90 pair.ProxyB == proxy)
91 {
92 CleanOverlappingPair(ref pair);
93 _overlappingPairs[i] = pair;
94 }
95 }
96 }
97
98 public void RemoveOverlappingPairsContainingProxy(BroadphaseProxy proxy)
99 {
100 for (int i = _overlappingPairs.Count - 1; i >= 0; i--)
101 {
102 BroadphasePair pair = _overlappingPairs[i];
103 if (pair.ProxyA == proxy ||
104 pair.ProxyB == proxy)
105 {
106 RemoveOverlappingPair(pair);
107 i++;
108 }
109 }
110 }
111
112 public bool NeedsBroadphaseCollision(BroadphaseProxy proxy0, BroadphaseProxy proxy1)
113 {
114 bool collides = (proxy0.CollisionFilterGroup & proxy1.CollisionFilterMask) != 0;
115 collides = collides && ((proxy1.CollisionFilterGroup & proxy0.CollisionFilterMask) != 0);
116
117 return collides;
118 }
119
120 public virtual void ProcessAllOverlappingPairs(IOverlapCallback callback)
121 {
122 List<BroadphasePair> deleting = new List<BroadphasePair>();
123 for (int i = 0; i < _overlappingPairs.Count; i++)
124 {
125 BroadphasePair p = _overlappingPairs[i];
126 if (callback.ProcessOverlap(ref p))
127 {
128 CleanOverlappingPair(ref p);
129 deleting.Add(p);
130 _overlappingPairCount--;
131 }
132 }
133
134 for (int i = 0; i < deleting.Count; i++)
135 _overlappingPairs.Remove(deleting[i]);
136 }
137
138 public void CleanOverlappingPair(ref BroadphasePair pair)
139 {
140 if (pair.CollisionAlgorithm != null)
141 {
142 if (pair.CollisionAlgorithm is IDisposable)
143 (pair.CollisionAlgorithm as IDisposable).Dispose();
144 pair.CollisionAlgorithm = null;
145 }
146 }
147
148 public abstract void RefreshOverlappingPairs();
149
150 #region IBroadphase Members
151 public abstract BroadphaseProxy CreateProxy(MonoXnaCompactMaths.Vector3 min, MonoXnaCompactMaths.Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask);
152
153 public abstract void DestroyProxy(BroadphaseProxy proxy);
154
155 public abstract void SetAabb(BroadphaseProxy proxy, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax);
156
157 #endregion
158 }
159}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs
new file mode 100644
index 0000000..1dc3f34
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphase.cs
@@ -0,0 +1,128 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class SimpleBroadphase : OverlappingPairCache
30 {
31 private int _maxProxies;
32 private List<SimpleBroadphaseProxy> _proxies = new List<SimpleBroadphaseProxy>();
33
34 public SimpleBroadphase()
35 : this(16384) { }
36
37 public SimpleBroadphase(int maxProxies)
38 : base()
39 {
40 _maxProxies = maxProxies;
41 }
42
43 public override BroadphaseProxy CreateProxy(Vector3 min, Vector3 max, BroadphaseNativeTypes shapeType, object userData, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask)
44 {
45 if (_proxies.Count >= _maxProxies)
46 {
47 BulletDebug.Assert(false);
48 return null; //should never happen, but don't let the game crash ;-)
49 }
50 BulletDebug.Assert(min.X <= max.X && min.Y <= max.Y && min.Z <= max.Z);
51
52 SimpleBroadphaseProxy proxy = new SimpleBroadphaseProxy(min, max, shapeType, userData, collisionFilterGroup, collisionFilterMask);
53 _proxies.Add(proxy);
54
55 return proxy;
56 }
57
58
59 public override void DestroyProxy(BroadphaseProxy proxy)
60 {
61 RemoveOverlappingPairsContainingProxy(proxy);
62 _proxies.Remove(proxy as SimpleBroadphaseProxy);
63 }
64
65 public override void SetAabb(BroadphaseProxy proxy, Vector3 aabbMin, Vector3 aabbMax)
66 {
67 SimpleBroadphaseProxy simpleProxy = GetSimpleProxyFromProxy(proxy);
68 simpleProxy.Minimum = aabbMin;
69 simpleProxy.Maximum = aabbMax;
70 }
71
72 private SimpleBroadphaseProxy GetSimpleProxyFromProxy(BroadphaseProxy proxy)
73 {
74 return proxy as SimpleBroadphaseProxy;
75 }
76
77 public override void RefreshOverlappingPairs()
78 {
79 for (int i = 0; i < _proxies.Count; i++)
80 {
81 SimpleBroadphaseProxy proxyA = _proxies[i];
82
83 for (int j = i + 1; j < _proxies.Count; j++)
84 {
85 SimpleBroadphaseProxy proxyB = _proxies[j];
86
87 if (AabbOverlap(proxyA, proxyB))
88 {
89 if (FindPair(proxyA, proxyB) == null)
90 {
91 AddOverlappingPair(proxyA, proxyB);
92 }
93 }
94 }
95 }
96
97 CheckOverlapCallback check = new CheckOverlapCallback();
98 ProcessAllOverlappingPairs(check);
99 }
100
101 public static bool AabbOverlap(SimpleBroadphaseProxy proxyA, SimpleBroadphaseProxy proxyB)
102 {
103 return proxyA.Minimum.X <= proxyB.Maximum.X && proxyB.Minimum.X <= proxyA.Maximum.X &&
104 proxyA.Minimum.Y <= proxyB.Maximum.Y && proxyB.Minimum.Y <= proxyA.Maximum.Y &&
105 proxyA.Minimum.Z <= proxyB.Maximum.Z && proxyB.Minimum.Z <= proxyA.Maximum.Z;
106 }
107
108 private void Validate()
109 {
110 for (int i = 0; i < _proxies.Count; i++)
111 {
112 for (int j = i + 1; j < _proxies.Count; j++)
113 {
114 if (_proxies[i] == _proxies[j])
115 throw new BulletException();
116 }
117 }
118 }
119 }
120
121 public class CheckOverlapCallback : IOverlapCallback
122 {
123 public bool ProcessOverlap(ref BroadphasePair pair)
124 {
125 return (!SimpleBroadphase.AabbOverlap(pair.ProxyA as SimpleBroadphaseProxy, pair.ProxyB as SimpleBroadphaseProxy));
126 }
127 }
128}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs
new file mode 100644
index 0000000..f473ef2
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/BroadphaseCollision/SimpleBroadphaseProxy.cs
@@ -0,0 +1,46 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class SimpleBroadphaseProxy : BroadphaseProxy
30 {
31 private Vector3 _min;
32 private Vector3 _max;
33
34 public SimpleBroadphaseProxy() { }
35
36 public SimpleBroadphaseProxy(Vector3 minPoint, Vector3 maxPoint, BroadphaseNativeTypes shapeType, object userData, CollisionFilterGroups collisionFilterGroup, CollisionFilterGroups collisionFilterMask)
37 : base(userData, collisionFilterGroup, collisionFilterMask)
38 {
39 _min = minPoint;
40 _max = maxPoint;
41 }
42
43 public Vector3 Minimum { get { return _min; } set { _min = value; } }
44 public Vector3 Maximum { get { return _max; } set { _max = value; } }
45 }
46}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs
new file mode 100644
index 0000000..ba638d1
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/BridgeTriangleRaycastCallback.cs
@@ -0,0 +1,59 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 internal class BridgeTriangleRaycastCallback : TriangleRaycastCallback
30 {
31 CollisionWorld.RayResultCallback _resultCallback;
32 CollisionObject _collisionObject;
33 TriangleMeshShape _triangleMesh;
34
35 public BridgeTriangleRaycastCallback(Vector3 from, Vector3 to,
36 CollisionWorld.RayResultCallback resultCallback, CollisionObject collisionObject, TriangleMeshShape triangleMesh)
37 : base(from, to)
38 {
39 _resultCallback = resultCallback;
40 _collisionObject = collisionObject;
41 _triangleMesh = triangleMesh;
42 }
43
44 public override float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex)
45 {
46 CollisionWorld.LocalShapeInfo shapeInfo = new CollisionWorld.LocalShapeInfo();
47 shapeInfo.ShapePart = partId;
48 shapeInfo.TriangleIndex = triangleIndex;
49
50 CollisionWorld.LocalRayResult rayResult = new CollisionWorld.LocalRayResult
51 (_collisionObject,
52 shapeInfo,
53 hitNormalLocal,
54 hitFraction);
55
56 return _resultCallback.AddSingleResult(rayResult);
57 }
58 }
59}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs
new file mode 100644
index 0000000..4e3903b
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionAlgorithmCreateFunc.cs
@@ -0,0 +1,40 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class CollisionAlgorithmCreateFunction
30 {
31 private bool _swapped;
32
33 public bool IsSwapped { get { return _swapped; } set { _swapped = value; } }
34
35 public virtual CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo ci, CollisionObject body0, CollisionObject body1)
36 {
37 return null;
38 }
39 }
40}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs
new file mode 100644
index 0000000..75d35b0
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionDispatcher.cs
@@ -0,0 +1,280 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public delegate void NearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo);
30
31 public class CollisionDispatcher : IDispatcher
32 {
33 private List<PersistentManifold> _manifolds = new List<PersistentManifold>();
34
35 //private bool _useIslands;
36 private NearCallback _nearCallback;
37
38 //private ManifoldResult _defaultManifoldResult;
39
40 private CollisionAlgorithmCreateFunction[,] _doubleDispatch = new CollisionAlgorithmCreateFunction[(int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes, (int)BroadphaseNativeTypes.MaxBroadphaseCollisionTypes];
41
42 //default CreationFunctions, filling the m_doubleDispatch table
43 private CollisionAlgorithmCreateFunction _convexConvexCreateFunc;
44 private CollisionAlgorithmCreateFunction _convexConcaveCreateFunc;
45 private CollisionAlgorithmCreateFunction _swappedConvexConcaveCreateFunc;
46 private CollisionAlgorithmCreateFunction _compoundCreateFunc;
47 private CollisionAlgorithmCreateFunction _swappedCompoundCreateFunc;
48 private CollisionAlgorithmCreateFunction _emptyCreateFunc;
49
50 private int _count;
51 private static int _manifoldCount = 0;
52
53 public CollisionDispatcher()
54 {
55 NearCallback = DefaultNearCallback;
56 //_useIslands = true;
57 //default CreationFunctions, filling the m_doubleDispatch table
58 _convexConvexCreateFunc = new ConvexConvexAlgorithm.CreateFunc();
59 _convexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.CreateFunc();
60 _swappedConvexConcaveCreateFunc = new ConvexConcaveCollisionAlgorithm.SwappedCreateFunc();
61 _compoundCreateFunc = new CompoundCollisionAlgorithm.CreateFunc();
62 _swappedCompoundCreateFunc = new CompoundCollisionAlgorithm.SwappedCreateFunc();
63 _emptyCreateFunc = new EmptyAlgorithm.CreateFunc();
64
65 for (BroadphaseNativeTypes i = BroadphaseNativeTypes.Box; i < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; i++)
66 {
67 for (BroadphaseNativeTypes j = BroadphaseNativeTypes.Box; j < BroadphaseNativeTypes.MaxBroadphaseCollisionTypes; j++)
68 {
69 _doubleDispatch[(int)i, (int)j] = FindCreateFunction(i, j);
70 if (_doubleDispatch[(int)i, (int)j] == null)
71 throw new BulletException();
72 }
73 }
74 }
75
76 public int Count { get { return _count; } set { _count = value; } }
77 public int ManifoldCount { get { return _manifolds.Count; } }
78 public List<PersistentManifold> Manifolds { get { return _manifolds; } }
79
80 public static int GlobalManifoldCount { get { return _manifoldCount; } set { _manifoldCount = value; } }
81
82 public PersistentManifold GetManifoldByIndex(int index)
83 {
84 return _manifolds[index];
85 }
86
87 //registerCollisionCreateFunc allows registration of custom/alternative collision create functions
88 public void RegisterCollisionCreateFunc(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB, CollisionAlgorithmCreateFunction createFunc)
89 {
90 _doubleDispatch[(int)proxyTypeA, (int)proxyTypeB] = createFunc;
91 }
92
93 public virtual PersistentManifold GetNewManifold(object bodyA, object bodyB)
94 {
95 _manifoldCount++;
96
97 CollisionObject body0 = bodyA as CollisionObject;
98 CollisionObject body1 = bodyB as CollisionObject;
99
100 PersistentManifold manifold = new PersistentManifold(body0, body1);
101 _manifolds.Add(manifold);
102
103 return manifold;
104 }
105
106 public virtual void ReleaseManifold(PersistentManifold manifold)
107 {
108 _manifoldCount--;
109
110 ClearManifold(manifold);
111 _manifolds.Remove(manifold);
112 }
113
114
115 public virtual void ClearManifold(PersistentManifold manifold)
116 {
117 manifold.ClearManifold();
118 }
119
120 public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB)
121 {
122 return FindAlgorithm(bodyA, bodyB, null);
123 }
124
125 public CollisionAlgorithm FindAlgorithm(CollisionObject bodyA, CollisionObject bodyB, PersistentManifold sharedManifold)
126 {
127 CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo();
128 collisionAlgorithmConstructionInfo.Dispatcher = this;
129 collisionAlgorithmConstructionInfo.Manifold = sharedManifold;
130 CollisionAlgorithm collisionAlgorithm = _doubleDispatch[(int)bodyA.CollisionShape.ShapeType, (int)bodyB.CollisionShape.ShapeType].CreateCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB);
131 return collisionAlgorithm;
132 }
133
134 /*public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1)
135 {
136 return internalFindAlgorithm(body0, body1, null);
137 }
138
139 public CollisionAlgorithm internalFindAlgorithm(CollisionObject body0, CollisionObject body1, PersistentManifold sharedManifold)
140 {
141 m_count++;
142
143 CollisionAlgorithmConstructionInfo ci = new CollisionAlgorithmConstructionInfo();
144 ci.m_dispatcher = this;
145
146 if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConvex())
147 {
148 return new ConvexConvexAlgorithm(sharedManifold, ci, body0, body1);
149 }
150
151 if (body0.getCollisionShape().isConvex() && body1.getCollisionShape().isConcave())
152 {
153 return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, false);
154 }
155
156 if (body1.getCollisionShape().isConvex() && body0.getCollisionShape().isConcave())
157 {
158 return new ConvexConcaveCollisionAlgorithm(ci, body0, body1, true);
159 }
160
161 if (body0.getCollisionShape().isCompound())
162 {
163 return new CompoundCollisionAlgorithm(ci, body0, body1, false);
164 }
165 else
166 {
167 if (body1.getCollisionShape().isCompound())
168 {
169 return new CompoundCollisionAlgorithm(ci, body0, body1, true);
170 }
171 }
172
173 //failed to find an algorithm
174 return new EmptyAlgorithm(ci);
175 }*/
176
177 public virtual bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB)
178 {
179 if (bodyA == null || bodyB == null)
180 throw new BulletException();
181
182 bool needsCollision = true;
183
184 //broadphase filtering already deals with this
185 /*if ((body0.isStaticObject() || body0.isKinematicObject()) &&
186 (body1.isStaticObject() || body1.isKinematicObject()))
187 {
188 printf("warning btCollisionDispatcher::needsCollision: static-static collision!\n");
189 }*/
190
191 if ((!bodyA.IsActive) && (!bodyB.IsActive))
192 needsCollision = false;
193
194 return needsCollision;
195 }
196
197 public virtual bool NeedsResponse(CollisionObject bodyA, CollisionObject bodyB)
198 {
199 //here you can do filtering
200 bool hasResponse = bodyA.HasContactResponse && bodyB.HasContactResponse;
201 hasResponse = hasResponse && (!bodyA.IsStaticOrKinematicObject || !bodyB.IsStaticOrKinematicObject);
202 return hasResponse;
203 }
204
205 public virtual void DispatchAllCollisionPairs(OverlappingPairCache pairCache, DispatcherInfo dispatchInfo)
206 {
207 CollisionPairCallback collisionCallback = new CollisionPairCallback(dispatchInfo, this);
208 pairCache.ProcessAllOverlappingPairs(collisionCallback);
209 }
210
211 private CollisionAlgorithmCreateFunction FindCreateFunction(BroadphaseNativeTypes proxyTypeA, BroadphaseNativeTypes proxyTypeB)
212 {
213 if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConvex(proxyTypeB))
214 {
215 return _convexConvexCreateFunc;
216 }
217
218 if (BroadphaseProxy.IsConvex(proxyTypeA) && BroadphaseProxy.IsConcave(proxyTypeB))
219 {
220 return _convexConcaveCreateFunc;
221 }
222
223 if (BroadphaseProxy.IsConvex(proxyTypeB) && BroadphaseProxy.IsConcave(proxyTypeA))
224 {
225 return _swappedConvexConcaveCreateFunc;
226 }
227
228 if (BroadphaseProxy.IsCompound(proxyTypeA))
229 {
230 return _compoundCreateFunc;
231 }
232 else
233 {
234 if (BroadphaseProxy.IsCompound(proxyTypeB))
235 {
236 return _swappedCompoundCreateFunc;
237 }
238 }
239
240 //failed to find an algorithm
241 return _emptyCreateFunc;
242 }
243
244 public NearCallback NearCallback { get { return _nearCallback; } set { _nearCallback = value; } }
245
246 //by default, Bullet will use this near callback
247 public static void DefaultNearCallback(ref BroadphasePair collisionPair, CollisionDispatcher dispatcher, DispatcherInfo dispatchInfo)
248 {
249 CollisionObject collisionObjectA = collisionPair.ProxyA.ClientData as CollisionObject;
250 CollisionObject collisionObjectB = collisionPair.ProxyB.ClientData as CollisionObject;
251
252 if (dispatcher.NeedsCollision(collisionObjectA, collisionObjectB))
253 {
254 //dispatcher will keep algorithms persistent in the collision pair
255 if (collisionPair.CollisionAlgorithm == null)
256 {
257 collisionPair.CollisionAlgorithm = dispatcher.FindAlgorithm(collisionObjectA, collisionObjectB);
258 }
259
260 if (collisionPair.CollisionAlgorithm != null)
261 {
262 ManifoldResult contactPointResult = new ManifoldResult(collisionObjectA, collisionObjectB);
263
264 if (dispatchInfo.DispatchFunction == DispatchFunction.Discrete)
265 {
266 //discrete collision detection query
267 collisionPair.CollisionAlgorithm.ProcessCollision(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult);
268 }
269 else
270 {
271 //continuous collision detection query, time of impact (toi)
272 float timeOfImpact = collisionPair.CollisionAlgorithm.CalculateTimeOfImpact(collisionObjectA, collisionObjectB, dispatchInfo, contactPointResult);
273 if (dispatchInfo.TimeOfImpact > timeOfImpact)
274 dispatchInfo.TimeOfImpact = timeOfImpact;
275 }
276 }
277 }
278 }
279 }
280}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs
new file mode 100644
index 0000000..4e9cf58
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionObject.cs
@@ -0,0 +1,163 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public enum ActivationState
30 {
31 Nothing = 0,
32 Active,
33 IslandSleeping,
34 WantsDeactivation,
35 DisableDeactivation,
36 DisableSimulation,
37 }
38
39 public enum CollisionOptions
40 {
41 StaticObject = 1,
42 KinematicObject = 2,
43 NoContactResponse = 4,
44 CustomMaterialCallback = 8,//this allows per-triangle material (friction/restitution)
45 }
46
47 /// <summary>
48 /// btCollisionObject can be used to manage collision detection objects.
49 /// btCollisionObject maintains all information that is needed for a collision detection: Shape, Transform and AABB proxy.
50 /// They can be added to the btCollisionWorld.
51 /// </summary>
52 public class CollisionObject
53 {
54 protected Matrix _worldTransform;
55 private BroadphaseProxy _broadphase;
56 private CollisionShape _collisionShape;
57
58 //m_interpolationWorldTransform is used for CCD and interpolation
59 //it can be either previous or future (predicted) transform
60 private Matrix _interpolationWorldTransform;
61
62 private CollisionOptions _collisionFlags;
63
64 private int _islandTag;
65 private ActivationState _activationState;
66 private float _deactivationTime;
67
68 private float _friction;
69 private float _restitution;
70
71 //users can point to their objects, m_userPointer is not used by Bullet
72 private object _userData;
73
74 //m_internalOwner one is used by optional Bullet high level interface
75 private object _internalOwner;
76
77 //time of impact calculation
78 private float _hitFraction;
79
80 //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm::
81 private float _ccdSweptSphereRadius;
82
83 // Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionTreshold
84 private float _ccdSquareMotionThreshold;
85 //those two are experimental: just added for bullet time effect, so you can still apply impulses (directly modifying velocities)
86 //without destroying the continuous interpolated motion (which uses this interpolation velocities)
87 private Vector3 _interpolationLinearVelocity;
88 private Vector3 _interpolationAngularVelocity;
89
90 private int _companionID;
91
92 public CollisionObject()
93 {
94 _activationState = ActivationState.Active;
95 _hitFraction = 1;
96 }
97
98 public bool IsStaticObject { get { return (_collisionFlags & CollisionOptions.StaticObject) != 0; } }
99 public bool IsKinematicObject { get { return (_collisionFlags & CollisionOptions.KinematicObject) != 0; } }
100 public bool IsStaticOrKinematicObject { get { return (_collisionFlags & (CollisionOptions.KinematicObject | CollisionOptions.StaticObject)) != 0; } }
101
102 public bool HasContactResponse { get { return (_collisionFlags & CollisionOptions.NoContactResponse) == 0; } }
103 public bool MergesSimulationIslands
104 {
105 get
106 {
107 //static objects, kinematic and object without contact response don't merge islands
108 return (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject | CollisionOptions.NoContactResponse)) == 0;
109 }
110 }
111
112 public ActivationState ActivationState
113 {
114 get { return _activationState; }
115 set
116 {
117 if ((_activationState != ActivationState.DisableDeactivation) && (_activationState != ActivationState.DisableSimulation))
118 _activationState = value;
119 }
120 }
121
122 public bool IsActive { get { return ((ActivationState != ActivationState.IslandSleeping) && (ActivationState != ActivationState.DisableSimulation)); } }
123 public float Restitution { get { return _restitution; } set { _restitution = value; } }
124 public float Friction { get { return _friction; } set { _friction = value; } }
125 public CollisionShape CollisionShape { get { return _collisionShape; } set { _collisionShape = value; } }
126 public float DeactivationTime { get { return _deactivationTime; } set { _deactivationTime = value; } }
127 public object Owner { get { return _internalOwner; } protected set { _internalOwner = value; } }
128 public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } }
129 public BroadphaseProxy Broadphase { get { return _broadphase; } set { _broadphase = value; } }
130 public Matrix InterpolationWorldTransform { get { return _interpolationWorldTransform; } set { _interpolationWorldTransform = value; } }
131 public Vector3 InterpolationLinearVelocity { get { return _interpolationLinearVelocity; } protected set { _interpolationLinearVelocity = value; } }
132 public Vector3 InterpolationAngularVelocity { get { return _interpolationAngularVelocity; } protected set { _interpolationAngularVelocity = value; } }
133 public int IslandTag { get { return _islandTag; } set { _islandTag = value; } }
134 public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } }
135 public CollisionOptions CollisionFlags { get { return _collisionFlags; } set { _collisionFlags = value; } }
136 //Swept sphere radius (0.0 by default), see btConvexConvexAlgorithm
137 public float CcdSweptSphereRadius { get { return _ccdSweptSphereRadius; } set { _ccdSweptSphereRadius = value; } }
138 // Don't do continuous collision detection if square motion (in one step) is less then m_ccdSquareMotionThreshold
139 public float CcdSquareMotionThreshold { get { return _ccdSquareMotionThreshold; } set { _ccdSquareMotionThreshold = value; } }
140 //users can point to their objects, userPointer is not used by Bullet
141 public object UserData { get { return _userData; } set { _userData = value; } }
142 public int CompanionID { get { return _companionID; } set { _companionID = value; } }
143
144 public void ForceActivationState(ActivationState newState)
145 {
146 _activationState = newState;
147 }
148
149 public void Activate()
150 {
151 Activate(false);
152 }
153
154 public void Activate(bool forceActivation)
155 {
156 if (forceActivation || (_collisionFlags & (CollisionOptions.StaticObject | CollisionOptions.KinematicObject)) == 0)
157 {
158 ActivationState = ActivationState.Active;
159 _deactivationTime = 0;
160 }
161 }
162 }
163}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs
new file mode 100644
index 0000000..7a85731
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionPairCallback.cs
@@ -0,0 +1,48 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class CollisionPairCallback : IOverlapCallback
30 {
31 private DispatcherInfo _dispatchInfo;
32 private CollisionDispatcher _dispatcher;
33
34 public CollisionPairCallback(DispatcherInfo dispatchInfo, CollisionDispatcher dispatcher)
35 {
36 _dispatchInfo = dispatchInfo;
37 _dispatcher = dispatcher;
38 }
39
40 #region IOverlapCallback Members
41 public bool ProcessOverlap(ref BroadphasePair pair)
42 {
43 _dispatcher.NearCallback(ref pair, _dispatcher, _dispatchInfo);
44 return false;
45 }
46 #endregion
47 }
48}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs
new file mode 100644
index 0000000..968e219
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CollisionWorld.cs
@@ -0,0 +1,358 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class CollisionWorld
30 {
31 private List<CollisionObject> _collisionObjects = new List<CollisionObject>();
32 private IDispatcher _dispatcher;
33 private OverlappingPairCache _broadphasePairCache;
34 private bool _ownsDispatcher;
35 private bool _ownsBroadphasePairCache;
36 private DispatcherInfo _dispatchInfo = new DispatcherInfo();
37
38 /// <summary>
39 /// this constructor doesn't own the dispatcher and paircache/broadphase
40 /// </summary>
41 /// <param name="dispatcher"></param>
42 /// <param name="pairCache"></param>
43 public CollisionWorld(IDispatcher dispatcher, OverlappingPairCache pairCache)
44 {
45 _dispatcher = dispatcher;
46 _broadphasePairCache = pairCache;
47 _ownsDispatcher = false;
48 _ownsBroadphasePairCache = false;
49 }
50
51 public DispatcherInfo DispatchInfo { get { return _dispatchInfo; } protected set { _dispatchInfo = value; } }
52 public List<CollisionObject> CollisionObjects { get { return _collisionObjects; } protected set { _collisionObjects = value; } }
53 public IBroadphase Broadphase { get { return _broadphasePairCache; } }
54 public OverlappingPairCache BroadphasePairCache { get { return _broadphasePairCache; } protected set { _broadphasePairCache = value; } }
55 public IDispatcher Dispatcher { get { return _dispatcher; } protected set { _dispatcher = value; } }
56 public int CollisionObjectsCount { get { return _collisionObjects.Count; } }
57 protected bool OwnsDispatcher { get { return _ownsDispatcher; } set { _ownsDispatcher = value; } }
58 protected bool OwnsBroadphasePairCache { get { return _ownsBroadphasePairCache; } set { _ownsBroadphasePairCache = value; } }
59
60 // rayTest performs a raycast on all objects in the btCollisionWorld, and calls the resultCallback
61 // This allows for several queries: first hit, all hits, any hit, dependent on the value returned by the callback.
62 public void RayTest(Vector3 rayFromWorld, Vector3 rayToWorld, RayResultCallback resultCallback)
63 {
64 Matrix rayFromTrans, rayToTrans;
65
66 rayFromTrans = Matrix.Identity;
67 rayFromTrans.Translation = rayFromWorld;
68
69 rayToTrans = Matrix.Identity;
70 rayToTrans.Translation = rayToWorld;
71
72 // brute force go over all objects. Once there is a broadphase, use that, or
73 // add a raycast against aabb first.
74
75 foreach (CollisionObject collisionObject in _collisionObjects)
76 {
77 //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
78 Vector3 collisionObjectAabbMin, collisionObjectAabbMax;
79 collisionObject.CollisionShape.GetAabb(collisionObject.WorldTransform, out collisionObjectAabbMin, out collisionObjectAabbMax);
80
81 float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing
82 Vector3 hitNormal = new Vector3();
83
84 //if (MathHelper.TestAabbAgainstAabb2(rayAabbMin, rayAabbMax, collisionObjectAabbMin, collisionObjectAabbMax))
85 if (MathHelper.RayAabb(rayFromWorld, rayToWorld, collisionObjectAabbMin, collisionObjectAabbMax, hitLambda, hitNormal))
86 {
87 RayTestSingle(rayFromTrans, rayToTrans,
88 collisionObject, collisionObject.CollisionShape,
89 collisionObject.WorldTransform, resultCallback);
90
91 }
92 }
93 }
94
95 // rayTestSingle performs a raycast call and calls the resultCallback. It is used internally by rayTest.
96 // In a future implementation, we consider moving the ray test as a virtual method in CollisionShape.
97 // This allows more customization.
98 public static void RayTestSingle(Matrix rayFromTrans, Matrix rayToTrans,
99 CollisionObject collisionObject,
100 CollisionShape collisionShape,
101 Matrix colObjWorldTransform,
102 RayResultCallback resultCallback)
103 {
104 SphereShape pointShape=new SphereShape(0.0f);
105
106 if (collisionShape.IsConvex)
107 {
108 CastResult castResult = new CastResult();
109 castResult.Fraction = 1f;//??
110
111 ConvexShape convexShape = collisionShape as ConvexShape;
112 VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
113 SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, convexShape, simplexSolver);
114 //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
115 //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
116
117 if (convexCaster.CalcTimeOfImpact(rayFromTrans, rayToTrans, colObjWorldTransform, colObjWorldTransform, castResult))
118 {
119 //add hit
120 if (castResult.Normal.LengthSquared() > 0.0001f)
121 {
122 castResult.Normal.Normalize();
123 if (castResult.Fraction < resultCallback.ClosestHitFraction)
124 {
125
126 CollisionWorld.LocalRayResult localRayResult = new LocalRayResult
127 (
128 collisionObject,
129 new LocalShapeInfo(),
130 castResult.Normal,
131 castResult.Fraction
132 );
133
134 resultCallback.AddSingleResult(localRayResult);
135 }
136 }
137 }
138 else
139 {
140 if (collisionShape.IsConcave)
141 {
142
143 TriangleMeshShape triangleMesh = collisionShape as TriangleMeshShape;
144
145 Matrix worldTocollisionObject = MathHelper.InvertMatrix(colObjWorldTransform);
146
147 Vector3 rayFromLocal = Vector3.TransformNormal(rayFromTrans.Translation, worldTocollisionObject);
148 Vector3 rayToLocal = Vector3.TransformNormal(rayToTrans.Translation, worldTocollisionObject);
149
150 BridgeTriangleRaycastCallback rcb = new BridgeTriangleRaycastCallback(rayFromLocal, rayToLocal, resultCallback, collisionObject, triangleMesh);
151 rcb.HitFraction = resultCallback.ClosestHitFraction;
152
153 Vector3 rayAabbMinLocal = rayFromLocal;
154 MathHelper.SetMin(ref rayAabbMinLocal, rayToLocal);
155 Vector3 rayAabbMaxLocal = rayFromLocal;
156 MathHelper.SetMax(ref rayAabbMaxLocal, rayToLocal);
157
158 triangleMesh.ProcessAllTriangles(rcb, rayAabbMinLocal, rayAabbMaxLocal);
159 }
160 else
161 {
162 //todo: use AABB tree or other BVH acceleration structure!
163 if (collisionShape.IsCompound)
164 {
165 CompoundShape compoundShape = collisionShape as CompoundShape;
166 for (int i = 0; i < compoundShape.ChildShapeCount; i++)
167 {
168 Matrix childTrans = compoundShape.GetChildTransform(i);
169 CollisionShape childCollisionShape = compoundShape.GetChildShape(i);
170 Matrix childWorldTrans = colObjWorldTransform * childTrans;
171 RayTestSingle(rayFromTrans, rayToTrans,
172 collisionObject,
173 childCollisionShape,
174 childWorldTrans,
175 resultCallback);
176 }
177 }
178 }
179 }
180 }
181 }
182
183 public void AddCollisionObject(CollisionObject collisionObject, BroadphaseProxy.CollisionFilterGroups collisionFilterGroup, BroadphaseProxy.CollisionFilterGroups collisionFilterMask)
184 {
185 //check that the object isn't already added
186 if (!_collisionObjects.Contains(collisionObject))
187 {
188 _collisionObjects.Add(collisionObject);
189
190 //calculate new AABB
191 Matrix trans = collisionObject.WorldTransform;
192
193 Vector3 minAabb;
194 Vector3 maxAabb;
195 collisionObject.CollisionShape.GetAabb(trans, out minAabb, out maxAabb);
196
197 BroadphaseNativeTypes type = collisionObject.CollisionShape.ShapeType;
198 collisionObject.Broadphase = Broadphase.CreateProxy(
199 minAabb,
200 maxAabb,
201 type,
202 collisionObject,
203 collisionFilterGroup,
204 collisionFilterMask
205 );
206 }
207 }
208
209 public void AddCollisionObject(CollisionObject collisionObject)
210 {
211 AddCollisionObject(collisionObject, BroadphaseProxy.CollisionFilterGroups.Default, BroadphaseProxy.CollisionFilterGroups.Default);
212 }
213
214 public void RemoveCollisionObject(CollisionObject collisionObject)
215 {
216 BroadphaseProxy bp = collisionObject.Broadphase;
217 if (bp != null)
218 {
219 //
220 // only clear the cached algorithms
221 //
222 Broadphase.CleanProxyFromPairs(bp);
223 Broadphase.DestroyProxy(bp);
224 collisionObject.Broadphase = null;
225 }
226
227 _collisionObjects.Remove(collisionObject);
228 }
229
230 public virtual void PerformDiscreteCollisionDetection()
231 {
232 DispatcherInfo dispatchInfo = DispatchInfo;
233 //update aabb (of all moved objects)
234
235 Vector3 aabbMin, aabbMax;
236 for (int i = 0; i < _collisionObjects.Count; i++)
237 {
238 _collisionObjects[i].CollisionShape.GetAabb(_collisionObjects[i].WorldTransform, out aabbMin, out aabbMax);
239 _broadphasePairCache.SetAabb(_collisionObjects[i].Broadphase, aabbMin, aabbMax);
240 }
241
242 _broadphasePairCache.RefreshOverlappingPairs();
243
244 IDispatcher dispatcher = Dispatcher;
245 if (dispatcher != null)
246 dispatcher.DispatchAllCollisionPairs(_broadphasePairCache, dispatchInfo);
247 }
248
249 public void Dispose(bool disposing)
250 {
251 if (disposing)
252 {
253 //clean up remaining objects
254 foreach (CollisionObject collisionObject in _collisionObjects)
255 {
256 BroadphaseProxy bp = collisionObject.Broadphase;
257 if (bp != null)
258 {
259 //
260 // only clear the cached algorithms
261 //
262 Broadphase.CleanProxyFromPairs(bp);
263 Broadphase.DestroyProxy(bp);
264 }
265 }
266 }
267 }
268
269 /// <summary>
270 /// LocalShapeInfo gives extra information for complex shapes
271 /// Currently, only TriangleMeshShape is available, so it just contains triangleIndex and subpart
272 /// </summary>
273 public struct LocalShapeInfo
274 {
275 private int _shapePart;
276 private int _triangleIndex;
277
278 public int ShapePart { get { return _shapePart; } set { _shapePart = value; } }
279 public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } }
280 }
281
282 public struct LocalRayResult
283 {
284 private CollisionObject _collisionObject;
285 private LocalShapeInfo _localShapeInfo;
286 private Vector3 _hitNormalLocal;
287 private float _hitFraction;
288
289 public LocalRayResult(CollisionObject collisionObject,
290 LocalShapeInfo localShapeInfo,
291 Vector3 hitNormalLocal,
292 float hitFraction)
293 {
294 _collisionObject = collisionObject;
295 _localShapeInfo = localShapeInfo;
296 _hitNormalLocal = hitNormalLocal;
297 _hitFraction = hitFraction;
298 }
299
300 public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } }
301 public LocalShapeInfo LocalShapeInfo { get { return _localShapeInfo; } set { _localShapeInfo = value; } }
302 public Vector3 HitNormalLocal { get { return _hitNormalLocal; } set { _hitNormalLocal = value; } }
303 public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } }
304 }
305
306 /// <summary>
307 /// RayResultCallback is used to report new raycast results
308 /// </summary>
309 public abstract class RayResultCallback
310 {
311 private float _closestHitFraction;
312
313 public RayResultCallback()
314 {
315 _closestHitFraction = 1;
316 }
317
318 public float ClosestHitFraction { get { return _closestHitFraction; } set { _closestHitFraction = value; } }
319 public bool HasHit { get { return _closestHitFraction < 1; } }
320
321 public abstract float AddSingleResult(LocalRayResult rayResult);
322 }
323
324 public class ClosestRayResultCallback : RayResultCallback
325 {
326 private Vector3 _rayFromWorld;//used to calculate hitPointWorld from hitFraction
327 private Vector3 _rayToWorld;
328
329 private Vector3 _hitNormalWorld;
330 private Vector3 _hitPointWorld;
331 private CollisionObject _collisionObject;
332
333 public ClosestRayResultCallback(Vector3 rayFromWorld, Vector3 rayToWorld)
334 {
335 _rayFromWorld = rayFromWorld;
336 _rayToWorld = rayToWorld;
337 _collisionObject = null;
338 }
339
340 public Vector3 RayFromWorld { get { return _rayFromWorld; } set { _rayFromWorld = value; } }
341 public Vector3 RayToWorld { get { return _rayToWorld; } set { _rayToWorld = value; } }
342 public Vector3 HitNormalWorld { get { return _hitNormalWorld; } set { _hitNormalWorld = value; } }
343 public Vector3 HitPointWorld { get { return _hitPointWorld; } set { _hitPointWorld = value; } }
344 public CollisionObject CollisionObject { get { return _collisionObject; } set { _collisionObject = value; } }
345
346 public override float AddSingleResult(LocalRayResult rayResult)
347 {
348 //caller already does the filter on the m_closestHitFraction
349 //assert(rayResult.m_hitFraction <= m_closestHitFraction);
350 ClosestHitFraction = rayResult.HitFraction;
351 _collisionObject = rayResult.CollisionObject;
352 _hitNormalWorld = Vector3.TransformNormal(rayResult.HitNormalLocal, _collisionObject.WorldTransform);
353 MathHelper.SetInterpolate3(_rayFromWorld, _rayToWorld, rayResult.HitFraction, ref _hitPointWorld);
354 return rayResult.HitFraction;
355 }
356 }
357 }
358}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs
new file mode 100644
index 0000000..3fec7d0
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/CompoundCollisionAlgorithm.cs
@@ -0,0 +1,157 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using System.Diagnostics;
26using MonoXnaCompactMaths;
27
28namespace XnaDevRu.BulletX
29{
30 public class CompoundCollisionAlgorithm : CollisionAlgorithm
31 {
32 private List<CollisionAlgorithm> _childCollisionAlgorithms;
33 private bool _isSwapped;
34
35 public CompoundCollisionAlgorithm(
36 CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo,
37 CollisionObject bodyA,
38 CollisionObject bodyB, bool isSwapped)
39 : base(collisionAlgorithmConstructionInfo)
40 {
41 //Begin
42 _isSwapped = isSwapped;
43
44 CollisionObject collisionObject = isSwapped ? bodyB : bodyA;
45 CollisionObject otherObject = isSwapped ? bodyA : bodyB;
46
47 BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);
48
49 CompoundShape compoundShape = collisionObject.CollisionShape as CompoundShape;
50 int childrenNumber = compoundShape.ChildShapeCount;
51 int index = 0;
52
53 _childCollisionAlgorithms = new List<CollisionAlgorithm>(childrenNumber);
54
55 for (index = 0; index < childrenNumber; index++)
56 {
57 CollisionShape childShape = compoundShape.GetChildShape(index);
58 CollisionShape orgShape = collisionObject.CollisionShape;
59
60 collisionObject.CollisionShape = childShape;
61 _childCollisionAlgorithms[index] = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(collisionObject, otherObject);
62 collisionObject.CollisionShape = orgShape;
63 }
64 }
65
66 public override void ProcessCollision(
67 CollisionObject bodyA,
68 CollisionObject bodyB,
69 DispatcherInfo dispatchInfo, ManifoldResult resultOut)
70 {
71 //Begin
72
73 CollisionObject collisionObject = _isSwapped ? bodyB : bodyB;
74 CollisionObject otherObject = _isSwapped ? bodyA : bodyB;
75
76 //Debug.Assert(collisionObject.getCollisionShape().isCompound());
77 BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);
78
79 CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape;
80
81 int childrenNumber = _childCollisionAlgorithms.Count;
82
83 for (int i = 0; i < childrenNumber; i++)
84 {
85 CompoundShape childShape = compoundShape.GetChildShape(i) as CompoundShape;
86
87 Matrix orgTransform = collisionObject.WorldTransform;
88 CollisionShape orgShape = collisionObject.CollisionShape;
89
90 Matrix childTransform = compoundShape.GetChildTransform(i);
91 Matrix newChildWorld = orgTransform * childTransform;
92
93 collisionObject.WorldTransform = newChildWorld;
94 collisionObject.CollisionShape = childShape;
95 _childCollisionAlgorithms[i].ProcessCollision(collisionObject, otherObject, dispatchInfo, resultOut);
96
97 collisionObject.CollisionShape = orgShape;
98 collisionObject.WorldTransform = orgTransform;
99 }
100 }
101
102 public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
103 {
104 CollisionObject collisionObject = _isSwapped ? bodyB : bodyA;
105 CollisionObject otherObject = _isSwapped ? bodyA : bodyB;
106
107 BulletDebug.Assert(collisionObject.CollisionShape.IsCompound);
108
109 CompoundShape compoundShape = (CompoundShape)collisionObject.CollisionShape;
110
111 float hitFraction = 1.0f;
112
113 for (int i = 0; i < _childCollisionAlgorithms.Count; i++)
114 {
115 CollisionShape childShape = compoundShape.GetChildShape(i);
116
117 Matrix orgTransform = collisionObject.WorldTransform;
118 CollisionShape orgShape = collisionObject.CollisionShape;
119
120 Matrix childTransform = compoundShape.GetChildTransform(i);
121 Matrix newChildWorld = orgTransform * childTransform;
122 collisionObject.WorldTransform = newChildWorld;
123
124 collisionObject.CollisionShape = childShape;
125 float frac = _childCollisionAlgorithms[i].CalculateTimeOfImpact(
126 collisionObject, otherObject, dispatchInfo, resultOut
127 );
128
129 if (frac < hitFraction)
130 {
131 hitFraction = frac;
132 }
133
134 collisionObject.CollisionShape = orgShape;
135 collisionObject.WorldTransform = orgTransform;
136 }
137
138 return hitFraction;
139 }
140
141 public class CreateFunc : CollisionAlgorithmCreateFunction
142 {
143 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
144 {
145 return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false);
146 }
147 };
148
149 public class SwappedCreateFunc : CollisionAlgorithmCreateFunction
150 {
151 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
152 {
153 return new CompoundCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true);
154 }
155 };
156 }
157}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs
new file mode 100644
index 0000000..c4949f0
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConcaveCollisionAlgorithm.cs
@@ -0,0 +1,189 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class ConvexConcaveCollisionAlgorithm : CollisionAlgorithm
30 {
31 private bool _isSwapped;
32 private ConvexTriangleCallback _convexTriangleCallback;
33
34 public ConvexConcaveCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped)
35 : base(collisionAlgorithmConstructionInfo)
36 {
37 _isSwapped = isSwapped;
38 _convexTriangleCallback = new ConvexTriangleCallback(collisionAlgorithmConstructionInfo.Dispatcher, bodyA, bodyB, isSwapped);
39 }
40
41 public void ClearCache()
42 {
43 _convexTriangleCallback.ClearCache();
44 }
45
46 public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
47 {
48 CollisionObject convexBody = _isSwapped ? bodyB : bodyA;
49 CollisionObject triBody = _isSwapped ? bodyA : bodyB;
50
51 if (triBody.CollisionShape.IsConcave)
52 {
53 CollisionObject triOb = triBody;
54 ConcaveShape concaveShape = triOb.CollisionShape as ConcaveShape;
55
56 if (convexBody.CollisionShape.IsConvex)
57 {
58 float collisionMarginTriangle = concaveShape.Margin;
59
60 resultOut.SetPersistentManifold(_convexTriangleCallback.Manifold);
61 _convexTriangleCallback.SetTimeStepAndCounters(collisionMarginTriangle, dispatchInfo, resultOut);
62
63 //Disable persistency. previously, some older algorithm calculated all contacts in one go, so you can clear it here.
64 //m_dispatcher->clearManifold(m_btConvexTriangleCallback.m_manifoldPtr);
65
66 _convexTriangleCallback.Manifold.SetBodies(convexBody, triBody);
67 concaveShape.ProcessAllTriangles(_convexTriangleCallback, _convexTriangleCallback.AabbMin, _convexTriangleCallback.AabbMax);
68 }
69 }
70 }
71
72 public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
73 {
74 CollisionObject convexbody = _isSwapped ? bodyB : bodyA;
75 CollisionObject triBody = _isSwapped ? bodyA : bodyB;
76
77
78 //quick approximation using raycast, todo: hook up to the continuous collision detection (one of the btConvexCast)
79
80 //only perform CCD above a certain threshold, this prevents blocking on the long run
81 //because object in a blocked ccd state (hitfraction<1) get their linear velocity halved each frame...
82 float squareMot0 = (convexbody.InterpolationWorldTransform.Translation - convexbody.WorldTransform.Translation).LengthSquared();
83 if (squareMot0 < convexbody.CcdSquareMotionThreshold)
84 {
85 return 1;
86 }
87
88 Matrix triInv = MathHelper.InvertMatrix(triBody.WorldTransform);
89 Matrix convexFromLocal = triInv * convexbody.WorldTransform;
90 Matrix convexToLocal = triInv * convexbody.InterpolationWorldTransform;
91
92 if (triBody.CollisionShape.IsConcave)
93 {
94 Vector3 rayAabbMin = convexFromLocal.Translation;
95 MathHelper.SetMin(ref rayAabbMin, convexToLocal.Translation);
96 Vector3 rayAabbMax = convexFromLocal.Translation;
97 MathHelper.SetMax(ref rayAabbMax, convexToLocal.Translation);
98 float ccdRadius0 = convexbody.CcdSweptSphereRadius;
99 rayAabbMin -= new Vector3(ccdRadius0, ccdRadius0, ccdRadius0);
100 rayAabbMax += new Vector3(ccdRadius0, ccdRadius0, ccdRadius0);
101
102 float curHitFraction = 1f; //is this available?
103 LocalTriangleSphereCastCallback raycastCallback = new LocalTriangleSphereCastCallback(convexFromLocal, convexToLocal,
104 convexbody.CcdSweptSphereRadius, curHitFraction);
105
106 raycastCallback.HitFraction = convexbody.HitFraction;
107
108 CollisionObject concavebody = triBody;
109
110 ConcaveShape triangleMesh = concavebody.CollisionShape as ConcaveShape;
111
112 if (triangleMesh != null)
113 {
114 triangleMesh.ProcessAllTriangles(raycastCallback, rayAabbMin, rayAabbMax);
115 }
116
117 if (raycastCallback.HitFraction < convexbody.HitFraction)
118 {
119 convexbody.HitFraction = raycastCallback.HitFraction;
120 return raycastCallback.HitFraction;
121 }
122 }
123
124 return 1;
125 }
126
127 public class CreateFunc : CollisionAlgorithmCreateFunction
128 {
129 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
130 {
131 return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, false);
132 }
133 }
134
135 public class SwappedCreateFunc : CollisionAlgorithmCreateFunction
136 {
137 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
138 {
139 return new ConvexConcaveCollisionAlgorithm(collisionAlgorithmConstructionInfo, bodyA, bodyB, true);
140 }
141 }
142
143 private class LocalTriangleSphereCastCallback : ITriangleCallback
144 {
145 private Matrix _ccdSphereFromTrans;
146 private Matrix _ccdSphereToTrans;
147 private Matrix _meshTransform;
148
149 private float _ccdSphereRadius;
150 private float _hitFraction;
151
152 public LocalTriangleSphereCastCallback(Matrix from, Matrix to, float ccdSphereRadius, float hitFraction)
153 {
154 _ccdSphereFromTrans = from;
155 _ccdSphereToTrans = to;
156 _ccdSphereRadius = ccdSphereRadius;
157 _hitFraction = hitFraction;
158 }
159
160 public Matrix CcdSphereFromTrans { get { return _ccdSphereFromTrans; } set { _ccdSphereFromTrans = value; } }
161 public Matrix CcdSphereToTrans { get { return _ccdSphereToTrans; } set { _ccdSphereToTrans = value; } }
162 public Matrix MeshTransform { get { return _meshTransform; } set { _meshTransform = value; } }
163 public float CcdSphereRadius { get { return _ccdSphereRadius; } set { _ccdSphereRadius = value; } }
164 public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } }
165
166 public void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex)
167 {
168 //do a swept sphere for now
169 Matrix ident = Matrix.Identity;
170 CastResult castResult = new CastResult();
171 castResult.Fraction = _hitFraction;
172 SphereShape pointShape = new SphereShape(_ccdSphereRadius);
173 TriangleShape triShape = new TriangleShape(triangle[0], triangle[1], triangle[2]);
174 VoronoiSimplexSolver simplexSolver = new VoronoiSimplexSolver();
175 SubsimplexConvexCast convexCaster = new SubsimplexConvexCast(pointShape, triShape, simplexSolver);
176 //GjkConvexCast convexCaster(&pointShape,convexShape,&simplexSolver);
177 //ContinuousConvexCollision convexCaster(&pointShape,convexShape,&simplexSolver,0);
178 //local space?
179
180 if (convexCaster.CalcTimeOfImpact(_ccdSphereFromTrans, _ccdSphereToTrans,
181 ident, ident, castResult))
182 {
183 if (_hitFraction > castResult.Fraction)
184 _hitFraction = castResult.Fraction;
185 }
186 }
187 }
188 }
189}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs
new file mode 100644
index 0000000..8fa4837
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexConvexCollisionAlgorithm.cs
@@ -0,0 +1,193 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26namespace XnaDevRu.BulletX
27{
28 public class ConvexConvexAlgorithm : CollisionAlgorithm, IDisposable
29 {
30 private const bool DisableCcd = false;
31 private GjkPairDetector _gjkPairDetector;
32 private bool _ownManifold;
33 private PersistentManifold _manifold;
34 private bool _lowLevelOfDetail;
35
36 public ConvexConvexAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
37 : base(collisionAlgorithmConstructionInfo)
38 {
39 _gjkPairDetector = new GjkPairDetector(null, null, simplexSolver, penetrationDepthSolver);
40 _ownManifold = false;
41 _manifold = manifold;
42 _lowLevelOfDetail = false;
43 }
44
45 public bool LowLevelOfDetail { get { return _lowLevelOfDetail; } set { _lowLevelOfDetail = value; } }
46 public bool OwnManifold { get { return _ownManifold; } set { _ownManifold = value; } }
47 public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } }
48
49 public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
50 {
51 if (_manifold == null)
52 {
53 //swapped?
54 _manifold = Dispatcher.GetNewManifold(bodyA, bodyB);
55 _ownManifold = true;
56 }
57 resultOut.SetPersistentManifold(_manifold);
58
59 ConvexShape min0 = bodyA.CollisionShape as ConvexShape;
60 ConvexShape min1 = bodyB.CollisionShape as ConvexShape;
61
62 GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
63
64 //TODO: if (dispatchInfo.m_useContinuous)
65 _gjkPairDetector.setMinkowskiA(min0);
66 _gjkPairDetector.setMinkowskiB(min1);
67 input.MaximumDistanceSquared = min0.Margin + min1.Margin + PersistentManifold.ContactBreakingThreshold;
68 input.MaximumDistanceSquared *= input.MaximumDistanceSquared;
69
70 // input.m_maximumDistanceSquared = 1e30f;
71
72 input.TransformA = bodyA.WorldTransform;
73 input.TransformB = bodyB.WorldTransform;
74
75 _gjkPairDetector.GetClosestPoints(input, resultOut, dispatchInfo.DebugDraw);
76 }
77
78 public override float CalculateTimeOfImpact(CollisionObject colA, CollisionObject colB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
79 {
80 //Rather then checking ALL pairs, only calculate TOI when motion exceeds threshold
81
82 //Linear motion for one of objects needs to exceed m_ccdSquareMotionThreshold
83 //col0->m_worldTransform,
84 float resultFraction = 1f;
85
86 float squareMotA = (colA.InterpolationWorldTransform.Translation - colA.WorldTransform.Translation).LengthSquared();
87 float squareMotB = (colB.InterpolationWorldTransform.Translation - colB.WorldTransform.Translation).LengthSquared();
88
89 if (squareMotA < colA.CcdSquareMotionThreshold &&
90 squareMotB < colB.CcdSquareMotionThreshold)
91 return resultFraction;
92
93 if (DisableCcd)
94 return 1f;
95
96 //An adhoc way of testing the Continuous Collision Detection algorithms
97 //One object is approximated as a sphere, to simplify things
98 //Starting in penetration should report no time of impact
99 //For proper CCD, better accuracy and handling of 'allowed' penetration should be added
100 //also the mainloop of the physics should have a kind of toi queue (something like Brian Mirtich's application of Timewarp for Rigidbodies)
101
102 // Convex0 against sphere for Convex1
103 {
104 ConvexShape convexA = colA.CollisionShape as ConvexShape;
105
106 SphereShape sphereB = new SphereShape(colB.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
107 CastResult result = new CastResult();
108 VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
109 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
110 //Simplification, one object is simplified as a sphere
111 GjkConvexCast ccdB = new GjkConvexCast(convexA, sphereB, voronoiSimplex);
112 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
113 if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,
114 colB.WorldTransform, colB.InterpolationWorldTransform, result))
115 {
116 //store result.m_fraction in both bodies
117 if (colA.HitFraction > result.Fraction)
118 colA.HitFraction = result.Fraction;
119
120 if (colB.HitFraction > result.Fraction)
121 colB.HitFraction = result.Fraction;
122
123 if (resultFraction > result.Fraction)
124 resultFraction = result.Fraction;
125 }
126 }
127
128 // Sphere (for convex0) against Convex1
129 {
130 ConvexShape convexB = colB.CollisionShape as ConvexShape;
131
132 SphereShape sphereA = new SphereShape(colA.CcdSweptSphereRadius); //todo: allow non-zero sphere sizes, for better approximation
133 CastResult result = new CastResult();
134 VoronoiSimplexSolver voronoiSimplex = new VoronoiSimplexSolver();
135 //SubsimplexConvexCast ccd0(&sphere,min0,&voronoiSimplex);
136 ///Simplification, one object is simplified as a sphere
137 GjkConvexCast ccdB = new GjkConvexCast(sphereA, convexB, voronoiSimplex);
138 //ContinuousConvexCollision ccd(min0,min1,&voronoiSimplex,0);
139 if (ccdB.CalcTimeOfImpact(colA.WorldTransform, colA.InterpolationWorldTransform,
140 colB.WorldTransform, colB.InterpolationWorldTransform, result))
141 {
142 //store result.m_fraction in both bodies
143 if (colA.HitFraction > result.Fraction)
144 colA.HitFraction = result.Fraction;
145
146 if (colB.HitFraction > result.Fraction)
147 colB.HitFraction = result.Fraction;
148
149 if (resultFraction > result.Fraction)
150 resultFraction = result.Fraction;
151 }
152 }
153 return resultFraction;
154 }
155
156 public class CreateFunc : CollisionAlgorithmCreateFunction
157 {
158 private IConvexPenetrationDepthSolver _penetrationDepthSolver;
159 private ISimplexSolver _simplexSolver;
160 //private bool _ownsSolvers;
161
162 public CreateFunc()
163 {
164 //_ownsSolvers = true;
165 _simplexSolver = new VoronoiSimplexSolver();
166 _penetrationDepthSolver = new GjkEpaPenetrationDepthSolver();
167 }
168
169 public CreateFunc(ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
170 {
171 //_ownsSolvers = false;
172 _simplexSolver = simplexSolver;
173 _penetrationDepthSolver = penetrationDepthSolver;
174 }
175
176 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
177 {
178 return new ConvexConvexAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, _simplexSolver, _penetrationDepthSolver);
179 }
180 }
181
182 #region IDisposable Members
183 public void Dispose()
184 {
185 if (_ownManifold)
186 {
187 if (_manifold != null)
188 Dispatcher.ReleaseManifold(_manifold);
189 }
190 }
191 #endregion
192 }
193}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs
new file mode 100644
index 0000000..5355817
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ConvexTriangleCallback.cs
@@ -0,0 +1,130 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class ConvexTriangleCallback : ITriangleCallback, IDisposable
30 {
31 private CollisionObject _convexBody;
32 private CollisionObject _triBody;
33
34 private Vector3 _aabbMin;
35 private Vector3 _aabbMax;
36
37 private ManifoldResult _resultOut;
38
39 private IDispatcher _dispatcher;
40 private DispatcherInfo _dispatchInfo;
41 private float _collisionMarginTriangle;
42
43 private int _triangleCount;
44
45 private PersistentManifold _manifold;
46
47 public ConvexTriangleCallback(IDispatcher dispatcher, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped)
48 {
49 _dispatcher = dispatcher;
50 _dispatchInfo = null;
51 _convexBody = isSwapped ? bodyB : bodyA;
52 _triBody = isSwapped ? bodyA : bodyB;
53
54 // create the manifold from the dispatcher 'manifold pool'
55 _manifold = _dispatcher.GetNewManifold(_convexBody, _triBody);
56 ClearCache();
57 }
58
59 public Vector3 AabbMin { get { return _aabbMin; } }
60 public Vector3 AabbMax { get { return _aabbMax; } }
61 public int TriangleCount { get { return _triangleCount; } set { _triangleCount = value; } }
62 public PersistentManifold Manifold { get { return _manifold; } set { _manifold = value; } }
63
64 public void SetTimeStepAndCounters(float collisionMarginTriangle, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
65 {
66 _dispatchInfo = dispatchInfo;
67 _collisionMarginTriangle = collisionMarginTriangle;
68 _resultOut = resultOut;
69
70 //recalc aabbs
71 Matrix convexInTriangleSpace = MathHelper.InvertMatrix(_triBody.WorldTransform) * _convexBody.WorldTransform;
72 CollisionShape convexShape = _convexBody.CollisionShape;
73 //CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);
74 convexShape.GetAabb(convexInTriangleSpace, out _aabbMin, out _aabbMax);
75 float extraMargin = collisionMarginTriangle;
76 Vector3 extra = new Vector3(extraMargin, extraMargin, extraMargin);
77
78 _aabbMax += extra;
79 _aabbMin -= extra;
80 }
81
82 public void ClearCache()
83 {
84 _dispatcher.ClearManifold(_manifold);
85 }
86
87 #region ITriangleCallback Members
88 public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex)
89 {
90 //aabb filter is already applied!
91 CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo = new CollisionAlgorithmConstructionInfo();
92 collisionAlgorithmConstructionInfo.Dispatcher = _dispatcher;
93
94 CollisionObject collisionObject = _triBody;
95
96 //debug drawing of the overlapping triangles
97 /*if (m_dispatchInfoPtr && m_dispatchInfoPtr.m_debugDraw && m_dispatchInfoPtr->m_debugDraw->getDebugMode() > 0)
98 {
99 Vector3 color = new Vector3(255, 255, 0);
100 btTransform & tr = ob->WorldTransform;
101 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[0]), tr(triangle[1]), color);
102 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[1]), tr(triangle[2]), color);
103 m_dispatchInfoPtr->m_debugDraw->drawLine(tr(triangle[2]), tr(triangle[0]), color);
104 }*/
105
106 if (_convexBody.CollisionShape.IsConvex)
107 {
108 TriangleShape triangleShape = new TriangleShape(triangle[0], triangle[1], triangle[2]);
109 triangleShape.Margin=_collisionMarginTriangle;
110
111 CollisionShape tempShape = collisionObject.CollisionShape;
112 collisionObject.CollisionShape = triangleShape;
113
114 CollisionAlgorithm collisionAlgorithm = collisionAlgorithmConstructionInfo.Dispatcher.FindAlgorithm(_convexBody, _triBody, _manifold);
115
116 _resultOut.SetShapeIdentifiers(-1, -1, partID, triangleIndex);
117 collisionAlgorithm.ProcessCollision(_convexBody, _triBody, _dispatchInfo, _resultOut);
118 collisionObject.CollisionShape = tempShape;
119 }
120 }
121 #endregion
122 #region IDisposable Members
123 public void Dispose()
124 {
125 ClearCache();
126 _dispatcher.ReleaseManifold(_manifold);
127 }
128 #endregion
129 }
130}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs
new file mode 100644
index 0000000..13cb3d9
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/EmptyAlgorithm.cs
@@ -0,0 +1,52 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26namespace XnaDevRu.BulletX
27{
28 /// <summary>
29 /// EmptyAlgorithm is a stub for unsupported collision pairs.
30 /// The dispatcher can dispatch a persistent btEmptyAlgorithm to avoid a search every frame.
31 /// </summary>
32 public class EmptyAlgorithm : CollisionAlgorithm
33 {
34 public EmptyAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo)
35 : base(collisionAlgorithmConstructionInfo) { }
36
37 public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut) { }
38
39 public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
40 {
41 return 1f;
42 }
43
44 public class CreateFunc : CollisionAlgorithmCreateFunction
45 {
46 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
47 {
48 return new EmptyAlgorithm(collisionAlgorithmConstructionInfo);
49 }
50 };
51 }
52}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs
new file mode 100644
index 0000000..63f37ea
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/ManifoldResult.cs
@@ -0,0 +1,147 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public delegate bool ContactAddedCallback(ManifoldPoint contactPoint, CollisionObject collisionObjectA, int partIdA, int indexA, CollisionObject collisionObjectB, int partIdB, int indexB);
30
31 public class ManifoldResult : DiscreteCollisionDetectorInterface.Result
32 {
33 private PersistentManifold _manifold;
34 private static ContactAddedCallback _contactAddedCallback = null;
35
36 //we need this for compounds
37 private Matrix _rootTransA;
38 private Matrix _rootTransB;
39
40 private CollisionObject _bodyA;
41 private CollisionObject _bodyB;
42 private int _partIdA;
43 private int _partIdB;
44 private int _indexA;
45 private int _indexB;
46
47 public ManifoldResult()
48 {
49 }
50
51 public ManifoldResult(CollisionObject bodyA, CollisionObject bodyB)
52 {
53 _bodyA = bodyA;
54 _bodyB = bodyB;
55 _rootTransA = bodyA.WorldTransform;
56 _rootTransB = bodyB.WorldTransform;
57 }
58
59 public static ContactAddedCallback ContactAddedCallback { get { return _contactAddedCallback; } set { _contactAddedCallback = value; } }
60
61 public void SetPersistentManifold(PersistentManifold manifold)
62 {
63 _manifold = manifold;
64 }
65
66 public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB)
67 {
68 _partIdA = partIdA;
69 _partIdB = partIdB;
70 _indexA = indexA;
71 _indexB = indexB;
72 }
73
74 public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth)
75 {
76 if (_manifold == null)
77 throw new BulletException("Manifold Pointer is null.");
78
79 //order in manifold needs to match
80
81 if (depth > PersistentManifold.ContactBreakingThreshold)
82 return;
83
84 bool isSwapped = _manifold.BodyA != _bodyA;
85
86 Vector3 pointA = pointInWorld + normalOnBInWorld * depth;
87 Vector3 localA;
88 Vector3 localB;
89
90 if (isSwapped)
91 {
92 localA = MathHelper.InvXForm(_rootTransB, pointA);
93 localB = MathHelper.InvXForm(_rootTransA, pointInWorld);
94 }
95 else
96 {
97 localA = MathHelper.InvXForm(_rootTransA, pointA);
98 localB = MathHelper.InvXForm(_rootTransB, pointInWorld);
99 }
100
101 ManifoldPoint newPt = new ManifoldPoint(localA, localB, normalOnBInWorld, depth);
102
103 int insertIndex = _manifold.GetCacheEntry(newPt);
104
105 newPt.CombinedFriction = CalculateCombinedFriction(_bodyA, _bodyB);
106 newPt.CombinedRestitution = CalculateCombinedRestitution(_bodyA, _bodyB);
107
108 //User can override friction and/or restitution
109 if (_contactAddedCallback != null &&
110 //and if either of the two bodies requires custom material
111 ((_bodyA.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0 ||
112 (_bodyB.CollisionFlags & CollisionOptions.CustomMaterialCallback) != 0))
113 {
114 //experimental feature info, for per-triangle material etc.
115 CollisionObject obj0 = isSwapped ? _bodyB : _bodyA;
116 CollisionObject obj1 = isSwapped ? _bodyA : _bodyB;
117 _contactAddedCallback(newPt, obj0, _partIdA, _indexA, obj1, _partIdB, _indexB);
118 }
119
120 if (insertIndex >= 0)
121 {
122 _manifold.ReplaceContactPoint(newPt, insertIndex);
123 }
124 else
125 {
126 _manifold.AddManifoldPoint(newPt);
127 }
128 }
129
130 private float CalculateCombinedFriction(CollisionObject bodyA, CollisionObject bodyB)
131 {
132 float friction = bodyA.Friction * bodyB.Friction;
133
134 float MaxFriction = 10;
135 if (friction < -MaxFriction)
136 friction = -MaxFriction;
137 if (friction > MaxFriction)
138 friction = MaxFriction;
139 return friction;
140 }
141
142 private float CalculateCombinedRestitution(CollisionObject bodyA, CollisionObject bodyB)
143 {
144 return bodyA.Restitution * bodyB.Restitution;
145 }
146 }
147}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs
new file mode 100644
index 0000000..957843b
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SimulationIslandManager.cs
@@ -0,0 +1,304 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class SimulationIslandManager
30 {
31 private UnionFind _unionFind = new UnionFind();
32
33 public void InitUnionFind(int n)
34 {
35 _unionFind.Reset(n);
36 }
37
38 public UnionFind UnionFind { get { return _unionFind; } }
39
40 public virtual void UpdateActivationState(CollisionWorld world, IDispatcher dispatcher)
41 {
42 InitUnionFind(world.CollisionObjectsCount);
43
44 // put the index into m_controllers into m_tag
45 int index = 0;
46 for (int i = 0; i < world.CollisionObjects.Count; i++)
47 {
48 world.CollisionObjects[i].IslandTag = index;
49 world.CollisionObjects[i].HitFraction = 1;
50 world.CollisionObjects[i].CompanionID = -1;
51 index++;
52 }
53 // do the union find
54 FindUnions(dispatcher);
55 }
56
57 public virtual void StoreIslandActivationState(CollisionWorld world)
58 {
59 // put the islandId ('find' value) into m_tag
60 int index = 0;
61 for (int i = 0; i < world.CollisionObjects.Count; i++)
62 {
63 if (world.CollisionObjects[i].MergesSimulationIslands)
64 {
65 world.CollisionObjects[i].IslandTag = _unionFind.Find(index);
66 world.CollisionObjects[i].CompanionID = -1;
67 }
68 else
69 {
70 world.CollisionObjects[i].IslandTag = -1;
71 world.CollisionObjects[i].CompanionID = -2;
72 }
73 index++;
74 }
75 }
76
77 public void FindUnions(IDispatcher dispatcher)
78 {
79 for (int i = 0; i < dispatcher.ManifoldCount; i++)
80 {
81 PersistentManifold manifold = dispatcher.GetManifoldByIndex(i);
82 //static objects (invmass 0.f) don't merge !
83
84 CollisionObject colObjA = manifold.BodyA as CollisionObject;
85 CollisionObject colObjB = manifold.BodyB as CollisionObject;
86
87 if (((colObjA != null) && (colObjA.MergesSimulationIslands)) &&
88 ((colObjB != null) && (colObjB.MergesSimulationIslands)))
89 {
90 _unionFind.Unite(colObjA.IslandTag, colObjB.IslandTag);
91 }
92 }
93 }
94
95 public void BuildAndProcessIslands(IDispatcher dispatcher, List<CollisionObject> collisionObjects, IIslandCallback callback)
96 {
97 //we are going to sort the unionfind array, and store the element id in the size
98 //afterwards, we clean unionfind, to make sure no-one uses it anymore
99 UnionFind.SortIslands();
100 int numElem = UnionFind.ElementCount;
101
102 int endIslandIndex = 1;
103 int startIslandIndex;
104
105 //update the sleeping state for bodies, if all are sleeping
106 for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
107 {
108 int islandId = UnionFind[startIslandIndex].ID;
109 for (endIslandIndex = startIslandIndex + 1; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++)
110 {
111 }
112
113 //int numSleeping = 0;
114
115 bool allSleeping = true;
116
117 int idx;
118 for (idx = startIslandIndex; idx < endIslandIndex; idx++)
119 {
120 int i = UnionFind[idx].Size;
121
122 CollisionObject colObjA = collisionObjects[i];
123 if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1))
124 {
125 Console.WriteLine("error in island management");
126 }
127
128 BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1));
129 if (colObjA.IslandTag == islandId)
130 {
131 if (colObjA.ActivationState == ActivationState.Active)
132 {
133 allSleeping = false;
134 }
135 if (colObjA.ActivationState == ActivationState.DisableDeactivation)
136 {
137 allSleeping = false;
138 }
139 }
140 }
141
142
143 if (allSleeping)
144 {
145 for (idx = startIslandIndex; idx < endIslandIndex; idx++)
146 {
147 int i = UnionFind[idx].Size;
148 CollisionObject colObjA = collisionObjects[i];
149 if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1))
150 {
151 Console.WriteLine("error in island management");
152 }
153
154 BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1));
155
156 if (colObjA.IslandTag == islandId)
157 {
158 colObjA.ActivationState =ActivationState.IslandSleeping;
159 }
160 }
161 }
162 else
163 {
164 for (idx = startIslandIndex; idx < endIslandIndex; idx++)
165 {
166 int i = UnionFind[idx].Size;
167
168 CollisionObject colObjA = collisionObjects[i];
169 if ((colObjA.IslandTag != islandId) && (colObjA.IslandTag != -1))
170 {
171 Console.WriteLine("error in island management");
172 }
173
174 BulletDebug.Assert((colObjA.IslandTag == islandId) || (colObjA.IslandTag == -1));
175
176 if (colObjA.IslandTag == islandId)
177 {
178 if (colObjA.ActivationState == ActivationState.IslandSleeping)
179 {
180 colObjA.ActivationState = ActivationState.WantsDeactivation;
181 }
182 }
183 }
184 }
185 }
186
187 //int maxNumManifolds = dispatcher.ManifoldCount;
188 List<PersistentManifold> islandmanifold = new List<PersistentManifold>(dispatcher.ManifoldCount);
189
190 for (int i = 0; i < dispatcher.ManifoldCount; i++)
191 {
192 PersistentManifold manifold = dispatcher.GetManifoldByIndex(i);
193
194 CollisionObject colObjA = manifold.BodyA as CollisionObject;
195 CollisionObject colObjB = manifold.BodyB as CollisionObject;
196
197 //todo: check sleeping conditions!
198 if (((colObjA != null) && colObjA.ActivationState != ActivationState.IslandSleeping) ||
199 ((colObjB != null) && colObjB.ActivationState != ActivationState.IslandSleeping))
200 {
201
202 //kinematic objects don't merge islands, but wake up all connected objects
203 if (colObjA.IsStaticOrKinematicObject && colObjA.ActivationState != ActivationState.IslandSleeping)
204 {
205 colObjB.Activate();
206 }
207 if (colObjB.IsStaticOrKinematicObject && colObjB.ActivationState != ActivationState.IslandSleeping)
208 {
209 colObjA.Activate();
210 }
211
212 //filtering for response
213 if (dispatcher.NeedsResponse(colObjA, colObjB))
214 islandmanifold.Add(manifold);
215 }
216 }
217
218 int numManifolds = islandmanifold.Count;
219
220 // Sort manifolds, based on islands
221 // Sort the vector using predicate and std::sort
222 islandmanifold.Sort(new Comparison<PersistentManifold>(PersistentManifoldSortPredicate));
223
224 //now process all active islands (sets of manifolds for now)
225 int startManifoldIndex = 0;
226 int endManifoldIndex = 1;
227
228 List<CollisionObject> islandBodies = new List<CollisionObject>();
229
230 for (startIslandIndex = 0; startIslandIndex < numElem; startIslandIndex = endIslandIndex)
231 {
232 int islandId = UnionFind[startIslandIndex].ID;
233 bool islandSleeping = false;
234 for (endIslandIndex = startIslandIndex; (endIslandIndex < numElem) && (UnionFind[endIslandIndex].ID == islandId); endIslandIndex++)
235 {
236 int i = UnionFind[endIslandIndex].Size;
237 CollisionObject colObjA = collisionObjects[i];
238 islandBodies.Add(colObjA);
239 if (!colObjA.IsActive)
240 islandSleeping = true;
241 }
242
243 //find the accompanying contact manifold for this islandId
244 int numIslandManifolds = 0;
245 List<PersistentManifold> startManifold = new List<PersistentManifold>(numIslandManifolds);
246
247 if (startManifoldIndex < numManifolds)
248 {
249 int curIslandID = GetIslandId(islandmanifold[startManifoldIndex]);
250 if (curIslandID == islandId)
251 {
252 for (int k = startManifoldIndex; k < islandmanifold.Count; k++)
253 {
254 startManifold.Add(islandmanifold[k]);
255 }
256 for (endManifoldIndex = startManifoldIndex + 1; (endManifoldIndex < numManifolds) && (islandId == GetIslandId(islandmanifold[endManifoldIndex])); endManifoldIndex++) { }
257
258 // Process the actual simulation, only if not sleeping/deactivated
259 numIslandManifolds = endManifoldIndex - startManifoldIndex;
260 }
261 }
262
263 if (!islandSleeping)
264 {
265 callback.ProcessIsland(islandBodies, startManifold, numIslandManifolds, islandId);
266 }
267
268 if (numIslandManifolds != 0)
269 {
270 startManifoldIndex = endManifoldIndex;
271 }
272
273 islandBodies.Clear();
274 }
275 }
276
277 private static int GetIslandId(PersistentManifold lhs)
278 {
279 int islandId;
280 CollisionObject rcolObjA = lhs.BodyA as CollisionObject;
281 CollisionObject rcolObjB = lhs.BodyB as CollisionObject;
282 islandId = rcolObjA.IslandTag >= 0 ? rcolObjA.IslandTag : rcolObjB.IslandTag;
283 return islandId;
284 }
285
286 private static int PersistentManifoldSortPredicate(PersistentManifold lhs, PersistentManifold rhs)
287 {
288 int rIslandIdA, lIslandIdB;
289 rIslandIdA = GetIslandId(rhs);
290 lIslandIdB = GetIslandId(lhs);
291 //return lIslandId0 < rIslandId0;
292 if (lIslandIdB < rIslandIdA)
293 return -1;
294 //else if (lIslandIdB > rIslandIdA)
295 // return 1;
296 return 1;
297 }
298
299 public interface IIslandCallback
300 {
301 void ProcessIsland(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, int islandID);
302 }
303 }
304}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs
new file mode 100644
index 0000000..8933638
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereBoxCollisionAlgorithm.cs
@@ -0,0 +1,270 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// SphereBoxCollisionAlgorithm provides sphere-box collision detection.
31 /// Other features are frame-coherency (persistent data) and collision response.
32 /// </summary>
33 public class SphereBoxCollisionAlgorithm : CollisionAlgorithm, IDisposable
34 {
35 private bool _ownManifold;
36 private PersistentManifold _manifold;
37 private bool _isSwapped;
38
39 public SphereBoxCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject collisionObjectA, CollisionObject collisionObjectB, bool isSwapped)
40 : base(collisionAlgorithmConstructionInfo)
41 {
42 _ownManifold = false;
43 _manifold = manifold;
44 _isSwapped = isSwapped;
45
46 CollisionObject sphereObject = _isSwapped ? collisionObjectB : collisionObjectA;
47 CollisionObject boxObject = _isSwapped ? collisionObjectA : collisionObjectB;
48
49 if (_manifold == null && Dispatcher.NeedsCollision(sphereObject, boxObject))
50 {
51 _manifold = Dispatcher.GetNewManifold(sphereObject, boxObject);
52 _ownManifold = true;
53 }
54 }
55
56 public float GetSphereDistance(CollisionObject boxObject, out Vector3 pointOnBox, out Vector3 pointOnSphere, Vector3 sphereCenter, float radius)
57 {
58 pointOnBox = new Vector3();
59 pointOnSphere = new Vector3();
60
61 float margins;
62 Vector3[] bounds = new Vector3[2];
63 BoxShape boxShape = boxObject.CollisionShape as BoxShape;
64
65 bounds[0] = -boxShape.HalfExtents;
66 bounds[1] = boxShape.HalfExtents;
67
68 margins = boxShape.Margin; //also add sphereShape margin?
69
70 Matrix m44T = boxObject.WorldTransform;
71
72 Vector3[] boundsVec = new Vector3[2];
73 float penetration;
74
75 boundsVec[0] = bounds[0];
76 boundsVec[1] = bounds[1];
77
78 Vector3 marginsVec = new Vector3(margins, margins, margins);
79
80 // add margins
81 bounds[0] += marginsVec;
82 bounds[1] -= marginsVec;
83
84 /////////////////////////////////////////////////
85
86 Vector3 tmp, prel, normal, v3P;
87 Vector3[] n = new Vector3[6];
88 float sep = 10000000.0f, sepThis;
89
90 n[0] = new Vector3(-1.0f, 0.0f, 0.0f);
91 n[1] = new Vector3(0.0f, -1.0f, 0.0f);
92 n[2] = new Vector3(0.0f, 0.0f, -1.0f);
93 n[3] = new Vector3(1.0f, 0.0f, 0.0f);
94 n[4] = new Vector3(0.0f, 1.0f, 0.0f);
95 n[5] = new Vector3(0.0f, 0.0f, 1.0f);
96
97 // convert point in local space
98 prel = MathHelper.InvXForm(m44T, sphereCenter);
99
100 bool found = false;
101
102 v3P = prel;
103
104 for (int i = 0; i < 6; i++)
105 {
106 int j = i < 3 ? 0 : 1;
107 if ((sepThis = (Vector3.Dot(v3P - bounds[j], n[i]))) > 0.0f)
108 {
109 v3P = v3P - n[i] * sepThis;
110 found = true;
111 }
112 }
113
114 //
115
116 if (found)
117 {
118 bounds[0] = boundsVec[0];
119 bounds[1] = boundsVec[1];
120
121 normal = Vector3.Normalize(prel - v3P);
122 pointOnBox = v3P + normal * margins;
123 pointOnSphere = prel - normal * radius;
124
125 if ((Vector3.Dot(pointOnSphere - pointOnBox, normal)) > 0.0f)
126 {
127 return 1.0f;
128 }
129
130 // transform back in world space
131 tmp = MathHelper.MatrixToVector(m44T, pointOnBox);
132 pointOnBox = tmp;
133 tmp = MathHelper.MatrixToVector(m44T, pointOnSphere);
134 pointOnSphere = tmp;
135 float seps2 = (pointOnBox - pointOnSphere).LengthSquared();
136
137 //if this fails, fallback into deeper penetration case, below
138 if (seps2 > MathHelper.Epsilon)
139 {
140 sep = -(float)Math.Sqrt(seps2);
141 normal = (pointOnBox - pointOnSphere);
142 normal *= 1f / sep;
143 }
144 return sep;
145 }
146
147 //////////////////////////////////////////////////
148 // Deep penetration case
149
150 penetration = GetSpherePenetration(boxObject, ref pointOnBox, ref pointOnSphere, sphereCenter, radius, bounds[0], bounds[1]);
151
152 bounds[0] = boundsVec[0];
153 bounds[1] = boundsVec[1];
154
155 if (penetration <= 0.0f)
156 return (penetration - margins);
157 else
158 return 1.0f;
159 }
160
161 public float GetSpherePenetration(CollisionObject boxObject, ref Vector3 pointOnBox, ref Vector3 pointOnSphere, Vector3 sphereCenter, float radius, Vector3 aabbMin, Vector3 aabbMax)
162 {
163 Vector3[] bounds = new Vector3[2];
164
165 bounds[0] = aabbMin;
166 bounds[1] = aabbMax;
167
168 Vector3 p0 = new Vector3(), tmp, prel, normal = new Vector3();
169 Vector3[] n = new Vector3[6];
170 float sep = -10000000.0f, sepThis;
171
172 n[0] = new Vector3(-1.0f, 0.0f, 0.0f);
173 n[1] = new Vector3(0.0f, -1.0f, 0.0f);
174 n[2] = new Vector3(0.0f, 0.0f, -1.0f);
175 n[3] = new Vector3(1.0f, 0.0f, 0.0f);
176 n[4] = new Vector3(0.0f, 1.0f, 0.0f);
177 n[5] = new Vector3(0.0f, 0.0f, 1.0f);
178
179 Matrix m44T = boxObject.WorldTransform;
180
181 // convert point in local space
182 prel = MathHelper.InvXForm(m44T, sphereCenter);
183
184 ///////////
185
186 for (int i = 0; i < 6; i++)
187 {
188 int j = i < 3 ? 0 : 1;
189 if ((sepThis = (Vector3.Dot(prel - bounds[j], n[i])) - radius) > 0.0f) return 1.0f;
190 if (sepThis > sep)
191 {
192 p0 = bounds[j];
193 normal = n[i];
194 sep = sepThis;
195 }
196 }
197
198 pointOnBox = prel - normal * (Vector3.Dot(normal, (prel - p0)));
199 pointOnSphere = pointOnBox + normal * sep;
200
201 // transform back in world space
202 tmp = MathHelper.MatrixToVector(m44T, pointOnBox);
203 pointOnBox = tmp;
204 tmp = MathHelper.MatrixToVector(m44T, pointOnSphere);
205 pointOnSphere = tmp;
206 normal = Vector3.Normalize(pointOnBox - pointOnSphere);
207
208 return sep;
209 }
210
211 public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
212 {
213 if (_manifold == null)
214 return;
215
216 CollisionObject sphereObject = _isSwapped ? bodyB : bodyA;
217 CollisionObject boxObject = _isSwapped ? bodyA : bodyB;
218
219 SphereShape sphereA = sphereObject.CollisionShape as SphereShape;
220
221 Vector3 pOnBox, pOnSphere;
222 Vector3 sphereCenter = sphereObject.WorldTransform.Translation;
223 float radius = sphereA.Radius;
224
225 float dist = GetSphereDistance(boxObject, out pOnBox, out pOnSphere, sphereCenter, radius);
226
227 if (dist < MathHelper.Epsilon)
228 {
229 Vector3 normalOnSurfaceB = Vector3.Normalize(pOnBox - pOnSphere);
230
231 // report a contact. internally this will be kept persistent, and contact reduction is done
232 resultOut.SetPersistentManifold(_manifold);
233 resultOut.AddContactPoint(normalOnSurfaceB, pOnBox, dist);
234 }
235 }
236
237 public override float CalculateTimeOfImpact(CollisionObject collisionObjectA, CollisionObject collisionObjectB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
238 {
239 //not yet
240 return 1;
241 }
242
243 public class CreateFunc : CollisionAlgorithmCreateFunction
244 {
245 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
246 {
247 if (!IsSwapped)
248 return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, false);
249 else
250 return new SphereBoxCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB, true);
251 }
252 }
253
254 #region IDisposable Members
255 public void Dispose()
256 {
257 Dispose(true);
258 }
259
260 public void Dispose(bool disposing)
261 {
262 if (disposing && _ownManifold)
263 {
264 if (_manifold != null)
265 Dispatcher.ReleaseManifold(_manifold);
266 }
267 }
268 #endregion
269 }
270}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs
new file mode 100644
index 0000000..7a76d24
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereSphereCollisionAlgorithm.cs
@@ -0,0 +1,104 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class SphereSphereCollisionAlgorithm : CollisionAlgorithm
30 {
31 private bool _ownManifold;
32 private PersistentManifold _manifold;
33
34 public SphereSphereCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
35 : base(collisionAlgorithmConstructionInfo)
36 {
37 _ownManifold = false;
38 _manifold = manifold;
39
40 if (_manifold == null)
41 {
42 _manifold = Dispatcher.GetNewManifold(bodyA, bodyB);
43 _ownManifold = true;
44 }
45 }
46
47 public SphereSphereCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo)
48 : base(collisionAlgorithmConstructionInfo) { }
49
50 ~SphereSphereCollisionAlgorithm()
51 {
52 if (_ownManifold)
53 {
54 if (_manifold != null)
55 Dispatcher.ReleaseManifold(_manifold);
56 }
57 }
58
59 public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
60 {
61 if (_manifold == null)
62 return;
63
64 SphereShape sphereA = bodyA.CollisionShape as SphereShape;
65 SphereShape sphereB = bodyB.CollisionShape as SphereShape;
66
67 Vector3 diff = bodyA.WorldTransform.Translation - bodyB.WorldTransform.Translation;
68 float len = diff.Length();
69 float radiusA = sphereA.Radius;
70 float radiusB = sphereB.Radius;
71
72 //if distance positive, don't generate a new contact
73 if (len > (radiusA + radiusB))
74 return;
75
76 //distance (negative means penetration)
77 float dist = len - (radiusA + radiusB);
78
79 Vector3 normalOnSurfaceB = diff / len;
80 //point on A (worldspace)
81 Vector3 posA = bodyA.WorldTransform.Translation - radiusA * normalOnSurfaceB;
82 //point on B (worldspace)
83 Vector3 posB = bodyB.WorldTransform.Translation + radiusB * normalOnSurfaceB;
84
85 // report a contact. internally this will be kept persistent, and contact reduction is done
86 resultOut.SetPersistentManifold(_manifold);
87 resultOut.AddContactPoint(normalOnSurfaceB, posB, dist);
88 }
89
90 public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
91 {
92 //not yet
93 return 1f;
94 }
95
96 public class CreateFunc : CollisionAlgorithmCreateFunction
97 {
98 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
99 {
100 return new SphereSphereCollisionAlgorithm(null, collisionAlgorithmConstructionInfo, bodyA, bodyB);
101 }
102 }
103 }
104}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs
new file mode 100644
index 0000000..1ca5cfb
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleCollisionAlgorithm.cs
@@ -0,0 +1,100 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// SphereSphereCollisionAlgorithm provides sphere-sphere collision detection.
31 /// Other features are frame-coherency (persistent data) and collision response.
32 /// Also provides the most basic sample for custom/user btCollisionAlgorithm
33 /// </summary>
34 public class SphereTriangleCollisionAlgorithm : CollisionAlgorithm, IDisposable
35 {
36 private bool _ownManifold;
37 private PersistentManifold _manifold;
38 private bool _isSwapped;
39
40 public SphereTriangleCollisionAlgorithm(PersistentManifold manifold, CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB, bool isSwapped)
41 : base(collisionAlgorithmConstructionInfo)
42 {
43 _ownManifold = false;
44 _manifold = manifold;
45 _isSwapped = isSwapped;
46
47 if (_manifold == null)
48 {
49 _manifold = Dispatcher.GetNewManifold(bodyA, bodyB);
50 _ownManifold = true;
51 }
52 }
53
54 public SphereTriangleCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo)
55 : base(collisionAlgorithmConstructionInfo) { }
56
57 public override void ProcessCollision(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
58 {
59 if (_manifold == null)
60 return;
61
62 SphereShape sphere = bodyA.CollisionShape as SphereShape;
63 TriangleShape triangle = bodyB.CollisionShape as TriangleShape;
64
65 /// report a contact. internally this will be kept persistent, and contact reduction is done
66 resultOut.SetPersistentManifold(_manifold);
67 SphereTriangleDetector detector = new SphereTriangleDetector(sphere, triangle);
68
69 DiscreteCollisionDetectorInterface.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
70 input.MaximumDistanceSquared = 1e30f;//todo: tighter bounds
71 input.TransformA = bodyA.WorldTransform;
72 input.TransformB = bodyB.WorldTransform;
73
74 detector.GetClosestPoints(input, resultOut, null);
75 }
76
77 public override float CalculateTimeOfImpact(CollisionObject bodyA, CollisionObject bodyB, DispatcherInfo dispatchInfo, ManifoldResult resultOut)
78 {
79 //not yet
80 return 1f;
81 }
82
83 public class CreateFunc : CollisionAlgorithmCreateFunction
84 {
85 public override CollisionAlgorithm CreateCollisionAlgorithm(CollisionAlgorithmConstructionInfo collisionAlgorithmConstructionInfo, CollisionObject bodyA, CollisionObject bodyB)
86 {
87 return new SphereTriangleCollisionAlgorithm(collisionAlgorithmConstructionInfo.Manifold, collisionAlgorithmConstructionInfo, bodyA, bodyB, IsSwapped);
88 }
89 }
90
91 #region IDisposable Members
92 public void Dispose()
93 {
94 if (_ownManifold)
95 if (_manifold != null)
96 Dispatcher.ReleaseManifold(_manifold);
97 }
98 #endregion
99 }
100}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs
new file mode 100644
index 0000000..865754a
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/SphereTriangleDetector.cs
@@ -0,0 +1,214 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class SphereTriangleDetector : DiscreteCollisionDetectorInterface
30 {
31 private SphereShape _sphere;
32 private TriangleShape _triangle;
33 private const int MaxOverlap = 0;
34
35 public SphereTriangleDetector(SphereShape sphere, TriangleShape triangle)
36 {
37 this._sphere = sphere;
38 this._triangle = triangle;
39 }
40
41 public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw)
42 {
43 Matrix transformA = input.TransformA;
44 Matrix transformB = input.TransformB;
45
46 Vector3 point = new Vector3();
47 Vector3 normal = new Vector3();
48 Single timeOfImpact = 1.0f;
49 Single depth = 0.0f;
50
51 //move sphere into triangle space
52 Matrix sphereInTr = MathHelper.InverseTimes(transformB, transformA);
53
54 if (Collide(sphereInTr.Translation, point, normal, depth, timeOfImpact))
55 output.AddContactPoint(Vector3.TransformNormal(normal, transformB), Vector3.TransformNormal(point, transformB), depth);
56 }
57
58 /// <summary>
59 /// See also geometrictools.com
60 /// Basic idea: D = |p - (lo + t0*lv)| where t0 = lv . (p - lo) / lv . lv
61 /// </summary>
62 /// <param name="from"></param>
63 /// <param name="to"></param>
64 /// <param name="p"></param>
65 /// <param name="nearest"></param>
66 /// <returns></returns>
67 private float SegmentSquareDistance(Vector3 from, Vector3 to, Vector3 point, Vector3 nearest)
68 {
69 Vector3 diff = point - from;
70 Vector3 v = to - from;
71 float t = Vector3.Dot(v, diff);
72
73 if (t > 0)
74 {
75 float dotVV = Vector3.Dot(v, v);
76 if (t < dotVV)
77 {
78 t /= dotVV;
79 diff -= t * v;
80 }
81 else
82 {
83 t = 1;
84 diff -= v;
85 }
86 }
87 else
88 t = 0;
89
90 nearest = from + t * v;
91 return Vector3.Dot(diff, diff);
92 }
93
94 private bool Collide(Vector3 sphereCenter, Vector3 point, Vector3 resultNormal, float depth, float timeOfImpact)
95 {
96 Vector3[] vertices = _triangle.Vertices;
97 Vector3 c = sphereCenter;
98 float r = _sphere.Radius;
99
100 Vector3 delta = new Vector3();
101
102 Vector3 normal = Vector3.Cross(vertices[1] - vertices[0], vertices[2] - vertices[0]);
103 normal = Vector3.Normalize(normal);
104 Vector3 p1ToCentre = c - vertices[0];
105 float distanceFromPlane = Vector3.Dot(p1ToCentre, normal);
106
107 if (distanceFromPlane < 0)
108 {
109 //triangle facing the other way
110 distanceFromPlane *= -1;
111 normal *= -1;
112 }
113
114 float contactMargin = PersistentManifold.ContactBreakingThreshold;
115 bool isInsideContactPlane = distanceFromPlane < r + contactMargin;
116 bool isInsideShellPlane = distanceFromPlane < r;
117
118 float deltaDotNormal = Vector3.Dot(delta, normal);
119 if (!isInsideShellPlane && deltaDotNormal >= 0.0f)
120 return false;
121
122 // Check for contact / intersection
123 bool hasContact = false;
124 Vector3 contactPoint = new Vector3();
125 if (isInsideContactPlane)
126 {
127 if (FaceContains(c, vertices, normal))
128 {
129 // Inside the contact wedge - touches a point on the shell plane
130 hasContact = true;
131 contactPoint = c - normal * distanceFromPlane;
132 }
133 else
134 {
135 // Could be inside one of the contact capsules
136 float contactCapsuleRadiusSqr = (r + contactMargin) * (r + contactMargin);
137 Vector3 nearestOnEdge = new Vector3();
138 for (int i = 0; i < _triangle.EdgeCount; i++)
139 {
140 Vector3 pa, pb;
141 _triangle.GetEdge(i, out pa, out pb);
142
143 float distanceSqr = SegmentSquareDistance(pa, pb, c, nearestOnEdge);
144 if (distanceSqr < contactCapsuleRadiusSqr)
145 {
146 // Yep, we're inside a capsule
147 hasContact = true;
148 contactPoint = nearestOnEdge;
149 }
150 }
151 }
152 }
153
154 if (hasContact)
155 {
156 Vector3 contactToCentre = c - contactPoint;
157 float distanceSqr = contactToCentre.LengthSquared();
158 if (distanceSqr < (r - MaxOverlap) * (r - MaxOverlap))
159 {
160 float distance = (float)Math.Sqrt(distanceSqr);
161 resultNormal = contactToCentre;
162 resultNormal = Vector3.Normalize(resultNormal);
163 point = contactPoint;
164 depth = -(r - distance);
165 return true;
166 }
167
168 if (Vector3.Dot(delta, contactToCentre) >= 0.0f)
169 return false;
170
171 // Moving towards the contact point -> collision
172 point = contactPoint;
173 timeOfImpact = 0.0f;
174 return true;
175 }
176 return false;
177 }
178
179 private bool PointInTriangle(Vector3[] vertices, Vector3 normal, Vector3 p)
180 {
181 Vector3 p1 = vertices[0];
182 Vector3 p2 = vertices[1];
183 Vector3 p3 = vertices[2];
184
185 Vector3 edge1 = p2 - p1;
186 Vector3 edge2 = p3 - p2;
187 Vector3 edge3 = p1 - p3;
188
189 Vector3 p1ToP = p - p1;
190 Vector3 p2ToP = p - p2;
191 Vector3 p3ToP = p - p3;
192
193 Vector3 edge1Normal = Vector3.Cross(edge1, normal);
194 Vector3 edge2Normal = Vector3.Cross(edge2, normal);
195 Vector3 edge3Normal = Vector3.Cross(edge3, normal);
196
197 float r1, r2, r3;
198 r1 = Vector3.Dot(edge1Normal, p1ToP);
199 r2 = Vector3.Dot(edge2Normal, p2ToP);
200 r3 = Vector3.Dot(edge3Normal, p3ToP);
201 if ((r1 > 0 && r2 > 0 && r3 > 0) ||
202 (r1 <= 0 && r2 <= 0 && r3 <= 0))
203 return true;
204 return false;
205 }
206
207 private bool FaceContains(Vector3 p, Vector3[] vertices, Vector3 normal)
208 {
209 Vector3 lp = p;
210 Vector3 lnormal = normal;
211 return PointInTriangle(vertices, lnormal, lp);
212 }
213 }
214} \ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs
new file mode 100644
index 0000000..a825fad
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionDispatch/UnionFind.cs
@@ -0,0 +1,151 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26namespace XnaDevRu.BulletX
27{
28 public class UnionFind : IDisposable
29 {
30 private List<Element> _elements = new List<Element>();
31
32 public int ElementCount
33 {
34 get { return _elements.Count; }
35 }
36
37 public void SortIslands()
38 {
39 for (int i = 0; i < _elements.Count; i++)
40 {
41 _elements[i].ID = Find(i);
42 _elements[i].Size = i;
43 }
44
45 _elements.Sort(Sort);
46 }
47
48 private static int Sort(Element x, Element y)
49 {
50 if (x.ID < y.ID) return -1;
51 //else if (x.ID > y.ID) return 1;
52 else return 0;
53 }
54
55 public void Reset(int number)
56 {
57 Allocate(number);
58
59 for (int i = 0; i < number; i++)
60 {
61 Element element = new Element();
62 element.ID = i;
63 element.Size = 1;
64 _elements.Insert(i, element);
65 }
66 }
67
68 public bool IsRoot(int index)
69 {
70 return (_elements[index].Size == index);
71 }
72
73 public Element this[int index]
74 {
75 get { return _elements[index]; }
76 }
77
78 public void Allocate(int number)
79 {
80 //Does nothing
81 _elements = new List<Element>(number);
82 }
83
84 public bool Find(int i, int j)
85 {
86 return (Find(i) == Find(j));
87 }
88
89 public int Find(int i)
90 {
91 while (i != _elements[i].ID)
92 {
93 //Element element = _elements[i];
94 //element.ID = _elements[_elements[i].ID].ID;
95 _elements[i].ID = _elements[_elements[i].ID].ID;
96 i = _elements[i].ID;
97 }
98
99 return i;
100 }
101
102 public void Unite(int p, int q)
103 {
104 int i = Find(p), j = Find(q);
105 if (i == j)
106 return;
107
108 //weighted quick union, this keeps the 'trees' balanced, and keeps performance of unite O( log(n) )
109 //if (_elements[i].Size < _elements[j].Size)
110 //{
111 // Element element = _elements[i];
112 // element.ID = j;
113 // _elements[i] = element;
114
115 // element = _elements[j];
116 // element.Size += _elements[i].Size;
117 // _elements[j] = element;
118 //}
119 //else
120 //{
121 // Element element = _elements[j];
122 // element.ID = i;
123 // _elements[j] = element;
124
125 // element = _elements[i];
126 // element.Size += _elements[j].Size;
127 // _elements[i] = element;
128 //}
129 _elements[i].ID = j;
130 _elements[j].Size += _elements[i].Size;
131 }
132
133 #region IDisposable Members
134
135 public void Dispose()
136 {
137 _elements.Clear();
138 }
139
140 #endregion
141 }
142
143 public class Element
144 {
145 private int _id;
146 private int _size;
147
148 public int ID { get { return _id; } set { _id = value; } }
149 public int Size { get { return _size; } set { _size = value; } }
150 }
151} \ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs
new file mode 100644
index 0000000..b75c2b6
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BUSimplex1to4.cs
@@ -0,0 +1,215 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// BUSimplex1to4 implements feature based and implicit simplex of up to 4 vertices (tetrahedron, triangle, line, vertex).
31 /// </summary>
32 public class BUSimplex1to4 : PolyhedralConvexShape
33 {
34 private int _numVertices = 0;
35 private Vector3[] _vertices = new Vector3[4];
36
37 public BUSimplex1to4() { }
38
39 public BUSimplex1to4(Vector3 pointA)
40 {
41 AddVertex(pointA);
42 }
43
44 public BUSimplex1to4(Vector3 pointA, Vector3 pointB)
45 {
46 AddVertex(pointA);
47 AddVertex(pointB);
48 }
49
50 public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC)
51 {
52 AddVertex(pointA);
53 AddVertex(pointB);
54 AddVertex(pointC);
55 }
56
57 public BUSimplex1to4(Vector3 pointA, Vector3 pointB, Vector3 pointC, Vector3 pointD)
58 {
59 AddVertex(pointA);
60 AddVertex(pointB);
61 AddVertex(pointC);
62 AddVertex(pointD);
63 }
64
65 protected Vector3[] Vertices { get { return _vertices; } set { _vertices = value; } }
66
67 public override int VertexCount
68 {
69 get
70 {
71 return _numVertices;
72 }
73 }
74
75 public override int EdgeCount
76 {
77 get
78 {
79 //euler formula, F-E+V = 2, so E = F+V-2
80 switch (_numVertices)
81 {
82 case 0: return 0;
83 case 1: return 0;
84 case 2: return 1;
85 case 3: return 3;
86 case 4: return 6;
87 }
88 return 0;
89 }
90 }
91
92 public override int PlaneCount
93 {
94 get
95 {
96 switch (_numVertices)
97 {
98 case 0:
99 return 0;
100 case 1:
101 return 0;
102 case 2:
103 return 0;
104 case 3:
105 return 2;
106 case 4:
107 return 4;
108 }
109 return 0;
110 }
111 }
112
113 public override BroadphaseNativeTypes ShapeType
114 {
115 get
116 {
117 return BroadphaseNativeTypes.Tetrahedral;
118 }
119 }
120
121 public override string Name
122 {
123 get
124 {
125 return "BUSimplex1to4";
126 }
127 }
128
129 public void AddVertex(Vector3 v)
130 {
131 _vertices[_numVertices++] = v;
132 }
133
134 public void Reset()
135 {
136 _numVertices = 0;
137 }
138
139 public override void GetEdge(int i, out Vector3 pa, out Vector3 pb)
140 {
141 switch (_numVertices)
142 {
143 case 2:
144 pa = _vertices[0];
145 pb = _vertices[1];
146 return;
147 case 3:
148 switch (i)
149 {
150 case 0:
151 pa = _vertices[0];
152 pb = _vertices[1];
153 return;
154 case 1:
155 pa = _vertices[1];
156 pb = _vertices[2];
157 return;
158 case 2:
159 pa = _vertices[2];
160 pb = _vertices[0];
161 return;
162 }
163 break;
164 case 4:
165 switch (i)
166 {
167 case 0:
168 pa = _vertices[0];
169 pb = _vertices[1];
170 return;
171 case 1:
172 pa = _vertices[1];
173 pb = _vertices[2];
174 return;
175 case 2:
176 pa = _vertices[2];
177 pb = _vertices[0];
178 return;
179 case 3:
180 pa = _vertices[0];
181 pb = _vertices[3];
182 return;
183 case 4:
184 pa = _vertices[1];
185 pb = _vertices[3];
186 return;
187 case 5:
188 pa = _vertices[2];
189 pb = _vertices[3];
190 return;
191 }
192 break;
193 }
194
195 pa = new Vector3();
196 pb = new Vector3();
197 }
198
199 public override void GetVertex(int i, out Vector3 vtx)
200 {
201 vtx = _vertices[i];
202 }
203
204 public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i)
205 {
206 planeNormal = new Vector3();
207 planeSupport = new Vector3();
208 }
209
210 public override bool IsInside(Vector3 pt, float tolerance)
211 {
212 return false;
213 }
214 }
215}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs
new file mode 100644
index 0000000..ba6f3b7
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BoxShape.cs
@@ -0,0 +1,316 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class BoxShape : PolyhedralConvexShape
30 {
31 public BoxShape(Vector3 boxHalfExtents)
32 {
33 ImplicitShapeDimensions = boxHalfExtents;
34 }
35
36 public override int VertexCount
37 {
38 get
39 {
40 return 8;
41 }
42 }
43
44 public override int EdgeCount
45 {
46 get
47 {
48 return 12;
49 }
50 }
51
52 public override BroadphaseNativeTypes ShapeType
53 {
54 get
55 {
56 return BroadphaseNativeTypes.Box;
57 }
58 }
59
60 public override string Name
61 {
62 get
63 {
64 return "Box";
65 }
66 }
67
68 public override int PreferredPenetrationDirectionsCount
69 {
70 get
71 {
72 return 6;
73 }
74 }
75
76 public override int PlaneCount
77 {
78 get
79 {
80 return 6;
81 }
82 }
83
84 public Vector3 HalfExtents { get { return ImplicitShapeDimensions * LocalScaling; } }
85
86 public override void GetEdge(int i, out Vector3 pa, out Vector3 pb)
87 {
88 int edgeVert0 = 0;
89 int edgeVert1 = 0;
90
91 switch (i)
92 {
93 case 0:
94 edgeVert0 = 0;
95 edgeVert1 = 1;
96 break;
97 case 1:
98 edgeVert0 = 0;
99 edgeVert1 = 2;
100 break;
101 case 2:
102 edgeVert0 = 1;
103 edgeVert1 = 3;
104
105 break;
106 case 3:
107 edgeVert0 = 2;
108 edgeVert1 = 3;
109 break;
110 case 4:
111 edgeVert0 = 0;
112 edgeVert1 = 4;
113 break;
114 case 5:
115 edgeVert0 = 1;
116 edgeVert1 = 5;
117
118 break;
119 case 6:
120 edgeVert0 = 2;
121 edgeVert1 = 6;
122 break;
123 case 7:
124 edgeVert0 = 3;
125 edgeVert1 = 7;
126 break;
127 case 8:
128 edgeVert0 = 4;
129 edgeVert1 = 5;
130 break;
131 case 9:
132 edgeVert0 = 4;
133 edgeVert1 = 6;
134 break;
135 case 10:
136 edgeVert0 = 5;
137 edgeVert1 = 7;
138 break;
139 case 11:
140 edgeVert0 = 6;
141 edgeVert1 = 7;
142 break;
143 default:
144 throw new BulletException();
145
146 }
147
148 GetVertex(edgeVert0, out pa);
149 GetVertex(edgeVert1, out pb);
150 }
151
152 public override void GetVertex(int i, out Vector3 vtx)
153 {
154 Vector3 halfExtents = HalfExtents;
155
156 vtx = new Vector3(
157 halfExtents.X * (1 - (i & 1)) - halfExtents.X * (i & 1),
158 halfExtents.Y * (1 - ((i & 2) >> 1)) - halfExtents.Y * ((i & 2) >> 1),
159 halfExtents.Z * (1 - ((i & 4) >> 2)) - halfExtents.Z * ((i & 4) >> 2));
160 }
161
162 public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i)
163 {
164 //this plane might not be aligned...
165 Vector4 plane;
166 GetPlaneEquation(out plane, i);
167 planeNormal = new Vector3(plane.X, plane.Y, plane.Z);
168 planeSupport = LocalGetSupportingVertex(-planeNormal);
169 }
170
171 public override bool IsInside(Vector3 pt, float tolerance)
172 {
173 Vector3 halfExtents = HalfExtents;
174
175 //btScalar minDist = 2*tolerance;
176
177 bool result = (pt.X <= ( halfExtents.X + tolerance)) &&
178 (pt.X >= (-halfExtents.X - tolerance)) &&
179 (pt.Y <= ( halfExtents.Y + tolerance)) &&
180 (pt.Y >= (-halfExtents.Y - tolerance)) &&
181 (pt.Z <= ( halfExtents.Z + tolerance)) &&
182 (pt.Z >= (-halfExtents.Z - tolerance));
183
184 return result;
185 }
186
187 public override Vector3 LocalGetSupportingVertex(Vector3 vec)
188 {
189 Vector3 halfExtents = HalfExtents;
190
191 return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X,
192 vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y,
193 vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z);
194 }
195
196 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
197 {
198 Vector3 halfExtents = HalfExtents;
199 Vector3 margin = new Vector3(Margin, Margin, Margin);
200 halfExtents -= margin;
201
202 return new Vector3( vec.X < 0.0f ? -halfExtents.X : halfExtents.X,
203 vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y,
204 vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z);
205 }
206
207 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
208 {
209 Vector3 halfExtents = HalfExtents;
210 Vector3 margin = new Vector3(Margin, Margin, Margin);
211 halfExtents -= margin;
212
213 for (int i = 0; i < vectors.Length; i++)
214 {
215 Vector3 vec = vectors[i];
216 supportVerticesOut[i] = new Vector3(vec.X < 0.0f ? -halfExtents.X : halfExtents.X,
217 vec.Y < 0.0f ? -halfExtents.Y : halfExtents.Y,
218 vec.Z < 0.0f ? -halfExtents.Z : halfExtents.Z);
219 }
220 }
221
222 public virtual void GetPlaneEquation(out Vector4 plane, int i)
223 {
224 Vector3 halfExtents = HalfExtents;
225
226 switch (i)
227 {
228 case 0:
229 plane = new Vector4(1, 0, 0, 0);
230 plane.W = -halfExtents.X;
231 break;
232 case 1:
233 plane = new Vector4(-1, 0, 0, 0);
234 plane.W = -halfExtents.X;
235 break;
236 case 2:
237 plane = new Vector4(0, 1, 0, 0);
238 plane.W = -halfExtents.Y;
239 break;
240 case 3:
241 plane = new Vector4(0, -1, 0, 0);
242 plane.W = -halfExtents.Y;
243 break;
244 case 4:
245 plane = new Vector4(0, 0, 1, 0);
246 plane.W = -halfExtents.Z;
247 break;
248 case 5:
249 plane = new Vector4(0, 0, -1, 0);
250 plane.W = -halfExtents.Z;
251 break;
252 default:
253 throw new BulletException();
254 }
255 }
256
257 public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector)
258 {
259 switch (index)
260 {
261 case 0:
262 penetrationVector = new Vector3(1, 0, 0);
263 break;
264 case 1:
265 penetrationVector = new Vector3(-1, 0, 0);
266 break;
267 case 2:
268 penetrationVector = new Vector3(0, 1, 0);
269 break;
270 case 3:
271 penetrationVector = new Vector3(0, -1, 0);
272 break;
273 case 4:
274 penetrationVector = new Vector3(0, 0, 1);
275 break;
276 case 5:
277 penetrationVector = new Vector3(0, 0, -1);
278 break;
279 default:
280 throw new BulletException();
281 }
282 }
283
284 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
285 {
286 Vector3 halfExtents = HalfExtents;
287
288 Matrix abs_b = MathHelper.Absolute(t);
289 Vector3 center = t.Translation;
290 Vector3 row1 = new Vector3(abs_b.M11, abs_b.M12, abs_b.M13);
291 Vector3 row2 = new Vector3(abs_b.M21, abs_b.M22, abs_b.M23);
292 Vector3 row3 = new Vector3(abs_b.M31, abs_b.M32, abs_b.M33);
293 Vector3 extent = new Vector3(Vector3.Dot(row1, halfExtents),
294 Vector3.Dot(row2, halfExtents),
295 Vector3.Dot(row3, halfExtents));
296 extent += new Vector3(Margin, Margin, Margin);
297
298 aabbMin = center - extent;
299 aabbMax = center + extent;
300 }
301
302 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
303 {
304 Vector3 halfExtents = HalfExtents;
305
306 float lx = 2f * (halfExtents.X);
307 float ly = 2f * (halfExtents.Y);
308 float lz = 2f * (halfExtents.Z);
309
310 inertia = new Vector3();
311 inertia.X = mass / (12.0f) * (ly * ly + lz * lz);
312 inertia.Y = mass / (12.0f) * (lx * lx + lz * lz);
313 inertia.Z = mass / (12.0f) * (lx * lx + ly * ly);
314 }
315 }
316}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs
new file mode 100644
index 0000000..00247a0
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/BvhTriangleMeshShape.cs
@@ -0,0 +1,83 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 class MyNodeOverlapCallback : INodeOverlapCallback
30 {
31 StridingMeshInterface _meshInterface;
32 ITriangleCallback _callback;
33 Vector3[] _triangle = new Vector3[3];
34
35 public MyNodeOverlapCallback(ITriangleCallback callback, StridingMeshInterface meshInterface)
36 {
37 _meshInterface = meshInterface;
38 _callback = callback;
39 }
40
41 public void ProcessNode(OptimizedBvhNode node)
42 {
43 List<Vector3> verts;
44 List<int> indicies;
45 int numtriangles;
46
47 _meshInterface.GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, node.SubPart);
48 Vector3 meshScaling = _meshInterface.Scaling;
49
50 for (int j = 0; j < 3; j++)
51 {
52 _triangle[j] = verts[indicies[j + node.TriangleIndex * 3]] * meshScaling;
53 }
54
55 _callback.ProcessTriangle(_triangle, node.SubPart, node.TriangleIndex);
56 _meshInterface.UnLockReadOnlyVertexBase(node.SubPart);
57 }
58 }
59
60 public class BvhTriangleMeshShape : TriangleMeshShape
61 {
62 OptimizedBvh _bvh = new OptimizedBvh();
63
64 public BvhTriangleMeshShape(StridingMeshInterface meshInterface) : base(meshInterface)
65 {
66 _bvh.Build(meshInterface);
67 }
68
69 public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
70 {
71 MyNodeOverlapCallback myNodeCallback = new MyNodeOverlapCallback(callback, MeshInterface);
72 _bvh.ReportAabbOverlappingNodex(myNodeCallback, aabbMin, aabbMax);
73 }
74
75 public override string Name
76 {
77 get
78 {
79 return "BvhTriangleMesh";
80 }
81 }
82 }
83}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs
new file mode 100644
index 0000000..a9ce3be
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CollisionShape.cs
@@ -0,0 +1,148 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// CollisionShape provides generic interface for collidable objects
31 /// </summary>
32 public abstract class CollisionShape
33 {
34 //debugging support
35 private string _tempDebug;
36
37 public abstract string Name { get; }
38 public string ExtraDebugInfo { get { return _tempDebug; } set { _tempDebug = value; } }
39
40 public bool IsPolyhedral
41 {
42 get
43 {
44 return BroadphaseProxy.IsPolyhedral(ShapeType);
45 }
46 }
47
48 public bool IsConvex
49 {
50 get
51 {
52 return BroadphaseProxy.IsConvex(ShapeType);
53 }
54 }
55 public bool IsConcave
56 {
57 get
58 {
59 return BroadphaseProxy.IsConcave(ShapeType);
60 }
61 }
62 public bool IsCompound
63 {
64 get
65 {
66 return BroadphaseProxy.IsCompound(ShapeType);
67 }
68 }
69
70 //isInfinite is used to catch simulation error (aabb check)
71 public bool IsInfinite
72 {
73 get
74 {
75 return BroadphaseProxy.IsInfinite(ShapeType);
76 }
77 }
78
79 public abstract float Margin { get; set; }
80 public abstract Vector3 LocalScaling { get; set; }
81 public abstract BroadphaseNativeTypes ShapeType { get; }
82
83
84 public virtual void GetBoundingSphere(out Vector3 center, out float radius)
85 {
86 Matrix tr = Matrix.Identity;
87 Vector3 aabbMin, aabbMax;
88
89 GetAabb(tr, out aabbMin, out aabbMax);
90
91 radius = (aabbMax - aabbMin).Length() * 0.5f;
92 center = (aabbMin + aabbMax) * 0.5f;
93 }
94
95 public virtual float GetAngularMotionDisc()
96 {
97 Vector3 center;
98 float disc;
99 GetBoundingSphere(out center, out disc);
100 disc += center.Length();
101 return disc;
102 }
103
104 //calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep)
105 //result is conservative
106 public void CalculateTemporalAabb(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, out Vector3 temporalAabbMin, out Vector3 temporalAabbMax)
107 {
108 //start with static aabb
109 GetAabb(currentTransform, out temporalAabbMin, out temporalAabbMax);
110
111 float temporalAabbMaxx = temporalAabbMax.X;
112 float temporalAabbMaxy = temporalAabbMax.Y;
113 float temporalAabbMaxz = temporalAabbMax.Z;
114 float temporalAabbMinx = temporalAabbMin.X;
115 float temporalAabbMiny = temporalAabbMin.Y;
116 float temporalAabbMinz = temporalAabbMin.Z;
117
118 // add linear motion
119 Vector3 linMotion = linearVelocity * timeStep;
120 //todo: simd would have a vector max/min operation, instead of per-element access
121 if (linMotion.X > 0)
122 temporalAabbMaxx += linMotion.X;
123 else
124 temporalAabbMinx += linMotion.X;
125 if (linMotion.Y > 0)
126 temporalAabbMaxy += linMotion.Y;
127 else
128 temporalAabbMiny += linMotion.Y;
129 if (linMotion.Z > 0)
130 temporalAabbMaxz += linMotion.Z;
131 else
132 temporalAabbMinz += linMotion.Z;
133
134 //add conservative angular motion
135 float angularMotion = angularVelocity.Length() * GetAngularMotionDisc() * timeStep;
136 Vector3 angularMotion3d = new Vector3(angularMotion, angularMotion, angularMotion);
137 temporalAabbMin = new Vector3(temporalAabbMinx, temporalAabbMiny, temporalAabbMinz);
138 temporalAabbMax = new Vector3(temporalAabbMaxx, temporalAabbMaxy, temporalAabbMaxz);
139
140 temporalAabbMin -= angularMotion3d;
141 temporalAabbMax += angularMotion3d;
142 }
143
144 public abstract void GetAabb(Matrix transform, out Vector3 aabbMin, out Vector3 aabbMax);
145
146 public abstract void CalculateLocalInertia(float mass, out Vector3 inertia);
147 }
148}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs
new file mode 100644
index 0000000..08aeec8
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CompoundShape.cs
@@ -0,0 +1,183 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// CompoundShape allows to store multiple other CollisionShapes
31 /// This allows for concave collision objects. This is more general then the Static Concave TriangleMeshShape.
32 /// </summary>
33 public class CompoundShape : CollisionShape
34 {
35 private List<Matrix> _childTransforms = new List<Matrix>();
36 private List<CollisionShape> _childShapes = new List<CollisionShape>();
37 private Vector3 _localAabbMin;
38 private Vector3 _localAabbMax;
39
40 private OptimizedBvh _aabbTree;
41 private float _collisionMargin;
42 private Vector3 _localScaling;
43
44 public CompoundShape()
45 {
46 _localAabbMin = new Vector3(1e30f, 1e30f, 1e30f);
47 _localAabbMax = new Vector3(-1e30f, -1e30f, -1e30f);
48 _aabbTree = null;
49 _collisionMargin = 0f;
50 _localScaling = new Vector3(1f, 1f, 1f);
51 }
52
53 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
54 {
55 Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin);
56 Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin);
57
58 Matrix abs_b = MathHelper.Absolute(t);
59
60 Vector3 row1 = new Vector3(abs_b.M11, abs_b.M12, abs_b.M13);
61 Vector3 row2 = new Vector3(abs_b.M21, abs_b.M22, abs_b.M23);
62 Vector3 row3 = new Vector3(abs_b.M31, abs_b.M32, abs_b.M33);
63
64 Vector3 center = new Vector3(Vector3.Dot(row1, localCenter) + t.Translation.X,
65 Vector3.Dot(row2, localCenter) + t.Translation.Y,
66 Vector3.Dot(row3, localCenter) + t.Translation.Z);
67
68 Vector3 extent = new Vector3(Vector3.Dot(row1, localHalfExtents),
69 Vector3.Dot(row2, localHalfExtents),
70 Vector3.Dot(row3, localHalfExtents));
71
72 aabbMin = center - extent;
73 aabbMax = center + extent;
74 }
75
76 public override BroadphaseNativeTypes ShapeType
77 {
78 get
79 {
80 return BroadphaseNativeTypes.Compound;
81 }
82 }
83
84 public override Vector3 LocalScaling
85 {
86 get
87 {
88 return _localScaling;
89 }
90 set
91 {
92 _localScaling = value;
93 }
94 }
95
96 public override string Name
97 {
98 get
99 {
100 return "Compound";
101 }
102 }
103
104 public override float Margin
105 {
106 get
107 {
108 return _collisionMargin;
109 }
110 set
111 {
112 _collisionMargin = value;
113 }
114 }
115
116 public int ChildShapeCount { get { return _childShapes.Count; } }
117 //this is optional, but should make collision queries faster, by culling non-overlapping nodes
118 public OptimizedBvh AabbTree { get { return _aabbTree; } }
119
120 public CollisionShape GetChildShape(int index)
121 {
122 return _childShapes[index];
123 }
124
125 public Matrix GetChildTransform(int index)
126 {
127 return _childTransforms[index];
128 }
129
130 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
131 {
132 //approximation: take the inertia from the aabb for now
133 Matrix ident = Matrix.Identity;
134 Vector3 aabbMin, aabbMax;
135 GetAabb(ident, out aabbMin, out aabbMax);
136
137 Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f;
138
139 float lx = 2f * (halfExtents.X);
140 float ly = 2f * (halfExtents.Y);
141 float lz = 2f * (halfExtents.Z);
142
143 inertia = new Vector3();
144 inertia.X = mass / (12.0f) * (ly * ly + lz * lz);
145 inertia.Y = mass / (12.0f) * (lx * lx + lz * lz);
146 inertia.Z = mass / (12.0f) * (lx * lx + ly * ly);
147 }
148
149 public void AddChildShape(Matrix localTransform, CollisionShape shape)
150 {
151 _childTransforms.Add(localTransform);
152 _childShapes.Add(shape);
153
154 //extend the local aabbMin/aabbMax
155 Vector3 localAabbMin, localAabbMax;
156 shape.GetAabb(localTransform, out localAabbMin, out localAabbMax);
157 if (_localAabbMin.X > localAabbMin.X)
158 {
159 _localAabbMin.X = localAabbMin.X;
160 }
161 if (_localAabbMax.X < localAabbMax.X)
162 {
163 _localAabbMax.X = localAabbMax.X;
164 }
165 if (_localAabbMin.Y > localAabbMin.Y)
166 {
167 _localAabbMin.Y = localAabbMin.Y;
168 }
169 if (_localAabbMax.Y < localAabbMax.Y)
170 {
171 _localAabbMax.Y = localAabbMax.Y;
172 }
173 if (_localAabbMin.Z > localAabbMin.Z)
174 {
175 _localAabbMin.Z = localAabbMin.Z;
176 }
177 if (_localAabbMax.Z < localAabbMax.Z)
178 {
179 _localAabbMax.Z = localAabbMax.Z;
180 }
181 }
182 }
183}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs
new file mode 100644
index 0000000..1e773f0
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConcaveShape.cs
@@ -0,0 +1,55 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public abstract class ConcaveShape : CollisionShape
30 {
31 private float _collisionMargin;
32
33 public ConcaveShape() { }
34
35 public float CollisionMargin
36 {
37 get { return _collisionMargin; }
38 set { _collisionMargin = value; }
39 }
40
41 public override float Margin
42 {
43 get
44 {
45 return _collisionMargin;
46 }
47 set
48 {
49 _collisionMargin = value;
50 }
51 }
52
53 public abstract void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax);
54 }
55} \ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs
new file mode 100644
index 0000000..361c277
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConeShape.cs
@@ -0,0 +1,208 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// ConeShape implements a Cone shape, around the X axis
31 /// </summary>
32 public class ConeShapeX : ConeShape
33 {
34 public ConeShapeX(float radius, float height)
35 : base(radius, height)
36 {
37 ConeUpIndex = 0;
38 }
39 }
40
41 /// <summary>
42 /// ConeShape implements a Cone shape, around the Z axis
43 /// </summary>
44 public class ConeShapeZ : ConeShape
45 {
46 public ConeShapeZ(float radius, float height)
47 : base(radius, height)
48 {
49 ConeUpIndex = 2;
50 }
51 }
52
53 /// <summary>
54 /// ConeShape implements a Cone shape, around the Y axis
55 /// </summary>
56 public class ConeShape : ConvexShape
57 {
58 private float _sinAngle;
59 private float _radius;
60 private float _height;
61 private int[] _coneIndices = new int[3];
62
63 public ConeShape(float radius, float height)
64 {
65 _radius = radius;
66 _height = height;
67 ConeUpIndex = 1;
68 _sinAngle = (_radius / (float)Math.Sqrt(_radius * _radius + _height * _height));
69 }
70
71 public float Radius { get { return _radius; } }
72 public float Height { get { return _height; } }
73
74 public override BroadphaseNativeTypes ShapeType
75 {
76 get
77 {
78 return BroadphaseNativeTypes.Cone;
79 }
80 }
81
82 public override string Name
83 {
84 get
85 {
86 return "Cone";
87 }
88 }
89
90 //choose upAxis index
91 public int ConeUpIndex
92 {
93 get { return _coneIndices[1]; }
94 set
95 {
96 switch (value)
97 {
98 case 0:
99 _coneIndices[0] = 1;
100 _coneIndices[1] = 0;
101 _coneIndices[2] = 2;
102 break;
103 case 1:
104 _coneIndices[0] = 0;
105 _coneIndices[1] = 1;
106 _coneIndices[2] = 2;
107 break;
108 case 2:
109 _coneIndices[0] = 0;
110 _coneIndices[1] = 2;
111 _coneIndices[2] = 1;
112 break;
113 default:
114 BulletDebug.Assert(false);
115 break;
116 }
117 }
118 }
119
120 private Vector3 ConeLocalSupport(Vector3 v)
121 {
122 float halfHeight = _height * 0.5f;
123 bool condition;
124
125 if (_coneIndices[1] == 0)
126 condition = v.X > v.Length() * _sinAngle;
127 else if (_coneIndices[1] == 1)
128 condition = v.Y > v.Length() * _sinAngle;
129 else
130 condition = v.Z > v.Length() * _sinAngle;
131
132 if (condition)
133 {
134 Vector3 tmp = new Vector3();
135 MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], halfHeight);
136 return tmp;
137 }
138 else
139 {
140 float s = (float)Math.Sqrt(MathHelper.GetValueByIndex(v, _coneIndices[0]) * MathHelper.GetValueByIndex(v, _coneIndices[0])
141 + MathHelper.GetValueByIndex(v, _coneIndices[2]) * MathHelper.GetValueByIndex(v, _coneIndices[2]));
142 if (s > MathHelper.Epsilon)
143 {
144 float d = _radius / s;
145 Vector3 tmp = new Vector3();
146 MathHelper.SetValueByIndex(ref tmp, _coneIndices[0], MathHelper.GetValueByIndex(v, _coneIndices[0]) * d);
147 MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight);
148 MathHelper.SetValueByIndex(ref tmp, _coneIndices[2], MathHelper.GetValueByIndex(v, _coneIndices[2]) * d);
149 return tmp;
150 }
151 else
152 {
153 Vector3 tmp = new Vector3();
154 MathHelper.SetValueByIndex(ref tmp, _coneIndices[1], -halfHeight);
155 return tmp;
156 }
157 }
158 }
159
160 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
161 {
162 return ConeLocalSupport(vec);
163 }
164
165 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
166 {
167 for (int i = 0; i < vectors.Length; i++)
168 supportVerticesOut[i] = ConeLocalSupport(vectors[i]);
169 }
170
171 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
172 {
173 Matrix identity = Matrix.Identity;
174 Vector3 aabbMin, aabbMax;
175 GetAabb(identity, out aabbMin, out aabbMax);
176
177 Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f;
178
179 float margin = Margin;
180
181 float lx = 2f * (halfExtents.X + margin);
182 float ly = 2f * (halfExtents.Y + margin);
183 float lz = 2f * (halfExtents.Z + margin);
184 float x2 = lx * lx;
185 float y2 = ly * ly;
186 float z2 = lz * lz;
187 float scaledmass = mass * 0.08333333f;
188
189 inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2));
190 }
191
192 public override Vector3 LocalGetSupportingVertex(Vector3 vec)
193 {
194 Vector3 supVertex = ConeLocalSupport(vec);
195 if (Margin != 0)
196 {
197 Vector3 vecnorm = vec;
198 if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))
199 {
200 vecnorm = new Vector3(-1f, -1f, -1f);
201 }
202 vecnorm = Vector3.Normalize(vecnorm);
203 supVertex += Margin * vecnorm;
204 }
205 return supVertex;
206 }
207 }
208}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs
new file mode 100644
index 0000000..eb78533
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexHullShape.cs
@@ -0,0 +1,184 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// ConvexHullShape implements an implicit (getSupportingVertex) Convex Hull of a Point Cloud (vertices)
31 /// No connectivity is needed. localGetSupportingVertex iterates linearly though all vertices.
32 /// on modern hardware, due to cache coherency this isn't that bad. Complex algorithms tend to trash the cash.
33 /// (memory is much slower then the cpu)
34 /// </summary>
35 public class ConvexHullShape : PolyhedralConvexShape
36 {
37 private List<Vector3> _points = new List<Vector3>();
38
39 public ConvexHullShape() { }
40
41 public override int VertexCount
42 {
43 get
44 {
45 return _points.Count;
46 }
47 }
48
49 public override int EdgeCount
50 {
51 get
52 {
53 return _points.Count;
54 }
55 }
56
57 public override int PlaneCount
58 {
59 get
60 {
61 return 0;
62 }
63 }
64
65 public override BroadphaseNativeTypes ShapeType
66 {
67 get
68 {
69 return BroadphaseNativeTypes.ConvexHull;
70 }
71 }
72
73 public override string Name
74 {
75 get
76 {
77 return "Convex";
78 }
79 }
80
81 public override Vector3 LocalGetSupportingVertex(Vector3 vec)
82 {
83 Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec);
84
85 if (Margin != 0)
86 {
87 Vector3 vecnorm = vec;
88 if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))
89 {
90 vecnorm=new Vector3(-1, -1, -1);
91 }
92 vecnorm = Vector3.Normalize(vecnorm);
93 supVertex += Margin * vecnorm;
94 }
95 return supVertex;
96 }
97
98 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0)
99 {
100 Vector3 supVec = new Vector3();
101 float newDot, maxDot = -1e30f;
102
103 Vector3 vec = vec0;
104 float lenSqr = vec.LengthSquared();
105 if (lenSqr < 0.0001f)
106 {
107 vec = new Vector3(1, 0, 0);
108 }
109 else
110 {
111 float rlen = 1f / (float)Math.Sqrt(lenSqr);
112 vec *= rlen;
113 }
114
115 for (int i = 0; i < _points.Count; i++)
116 {
117 Vector3 vtx = _points[i] * LocalScaling;
118
119 newDot = Vector3.Dot(vec, vtx);
120 if (newDot > maxDot)
121 {
122 maxDot = newDot;
123 supVec = vtx;
124 }
125 }
126 return supVec;
127 }
128
129 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
130 {
131 float newDot;
132 //use 'w' component of supportVerticesOut?
133 /*{
134 for (int i = 0; i < numVectors; i++)
135 {
136 supportVerticesOut[i][3] = -1e30f;
137 }
138 }*/
139 #warning Warning!
140 for (int i = 0; i < _points.Count; i++)
141 {
142 Vector3 vtx = _points[i] * LocalScaling;
143
144 for (int j = 0; j < vectors.Length; j++)
145 {
146 newDot = Vector3.Dot(vectors[j], vtx);
147 if (newDot > -1e30f)
148 {
149 //WARNING: don't swap next lines, the w component would get overwritten!
150 supportVerticesOut[j] = vtx;
151 //supportVerticesOut[j][3] = newDot;
152 #warning Warning!
153 }
154 }
155 }
156 }
157
158 public override void GetEdge(int i, out Vector3 pa, out Vector3 pb)
159 {
160 int index0 = i % _points.Count;
161 int index1 = (i + 1) % _points.Count;
162 pa = _points[index0] * LocalScaling;
163 pb = _points[index1] * LocalScaling;
164 }
165
166 public override void GetVertex(int i, out Vector3 vtx)
167 {
168 vtx = _points[i] * LocalScaling;
169 }
170
171 public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i)
172 {
173 planeNormal = new Vector3();
174 planeSupport = new Vector3();
175 BulletDebug.Assert(false);
176 }
177
178 public override bool IsInside(Vector3 pt, float tolerance)
179 {
180 BulletDebug.Assert(false);
181 return false;
182 }
183 }
184}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs
new file mode 100644
index 0000000..02d678e
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexShape.cs
@@ -0,0 +1,141 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// ConvexShape is an abstract shape interface.
31 /// The explicit part provides plane-equations, the implicit part provides GetClosestPoint interface.
32 /// used in combination with GJK or btConvexCast
33 /// </summary>
34 public abstract class ConvexShape : CollisionShape
35 {
36 private const int _maxPreferredPenetrationDirections = 10;
37 private const float _convexDistanceMargin = 0.04f;
38
39 private Vector3 _localScaling;
40 private Vector3 _implicitShapeDimensions;
41 private float _collisionMargin;
42
43 public ConvexShape()
44 : base()
45 {
46 _localScaling = Vector3.One;
47 _collisionMargin = ConvexDistanceMargin;
48 }
49
50 public static int MaxPreferredPenetrationDirections { get { return _maxPreferredPenetrationDirections; } }
51 public static float ConvexDistanceMargin { get { return _convexDistanceMargin; } }
52
53 public Vector3 ImplicitShapeDimensions { get { return _implicitShapeDimensions; } protected set { _implicitShapeDimensions = value; } }
54 public virtual int PreferredPenetrationDirectionsCount { get { return 0; } }
55
56 protected float CollisionMargin { get { return _collisionMargin; } set { _collisionMargin = value; } }
57
58 public virtual void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector)
59 {
60 penetrationVector = new Vector3();
61 BulletDebug.Assert(false);
62 }
63
64 public abstract Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec);
65 //notice that the vectors should be unit length
66 public abstract void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut);
67
68 /// <summary>
69 /// getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
70 /// </summary>
71 /// <param name="t"></param>
72 /// <param name="aabbMin"></param>
73 /// <param name="aabbMax"></param>
74 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
75 {
76 GetAabbSlow(t, out aabbMin, out aabbMax);
77 }
78
79 public override Vector3 LocalScaling
80 {
81 get
82 {
83 return _localScaling;
84 }
85 set
86 {
87 _localScaling = value;
88 }
89 }
90
91 public override float Margin
92 {
93 get
94 {
95 return _collisionMargin;
96 }
97 set
98 {
99 _collisionMargin = value;
100 }
101 }
102
103 public virtual Vector3 LocalGetSupportingVertex(Vector3 vec)
104 {
105 Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec);
106
107 if (Margin != 0f)
108 {
109 Vector3 vecnorm = vec;
110 if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))
111 {
112 vecnorm = new Vector3(-1f, -1f, -1f);
113 }
114 vecnorm.Normalize();
115 supVertex += Margin * vecnorm;
116 }
117 return supVertex;
118 }
119
120 public virtual void GetAabbSlow(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
121 {
122 float margin = Margin;
123 aabbMax = new Vector3();
124 aabbMin = new Vector3();
125
126 for (int i = 0; i < 3; i++)
127 {
128 Vector3 vec = new Vector3(0f, 0f, 0f);
129 MathHelper.SetElement(ref vec, i, 1);
130
131 Vector3 sv = LocalGetSupportingVertex(Vector3.TransformNormal(vec, t));
132
133 Vector3 tmp = MathHelper.MatrixToVector(t, sv);
134 MathHelper.SetElement(ref aabbMax, i, MathHelper.GetElement(tmp, i) + margin);
135 MathHelper.SetElement(ref vec, i, -1f);
136 tmp = MathHelper.MatrixToVector(t, LocalGetSupportingVertex(Vector3.TransformNormal(vec, t)));
137 MathHelper.SetElement(ref aabbMin, i, MathHelper.GetElement(tmp, i) - margin);
138 }
139 }
140 }
141}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs
new file mode 100644
index 0000000..0fd3fa7
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/ConvexTriangleMeshShape.cs
@@ -0,0 +1,185 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// ConvexTriangleMeshShape is a convex hull of a triangle mesh. If you just have a point cloud, you can use ConvexHullShape instead.
31 /// It uses the StridingMeshInterface instead of a point cloud. This can avoid the duplication of the triangle mesh data.
32 /// </summary>
33 public class ConvexTriangleMeshShape : PolyhedralConvexShape
34 {
35 private StridingMeshInterface _stridingMesh;
36
37 public ConvexTriangleMeshShape(StridingMeshInterface meshInterface)
38 {
39 _stridingMesh = meshInterface;
40 }
41
42 public StridingMeshInterface getStridingMesh()
43 {
44 return _stridingMesh;
45 }
46
47 public override int VertexCount
48 {
49 get
50 {
51 return 0;
52 }
53 }
54
55 public override int EdgeCount
56 {
57 get
58 {
59 return 0;
60 }
61 }
62
63 public override int PlaneCount
64 {
65 get
66 {
67 return 0;
68 }
69 }
70
71 public override Vector3 LocalScaling
72 {
73 get
74 {
75 return base.LocalScaling;
76 }
77 set
78 {
79 _stridingMesh.Scaling = value;
80 }
81 }
82
83 public override BroadphaseNativeTypes ShapeType
84 {
85 get
86 {
87 return BroadphaseNativeTypes.ConvexTriangleMesh;
88 }
89 }
90
91 public override string Name
92 {
93 get
94 {
95 return "ConvexTrimesh";
96 }
97 }
98
99 public override void GetEdge(int i, out Vector3 pa, out Vector3 pb)
100 {
101 pa = new Vector3();
102 pb = new Vector3();
103 BulletDebug.Assert(false);
104 }
105
106 public override void GetVertex(int i, out Vector3 vtx)
107 {
108 vtx = new Vector3();
109 BulletDebug.Assert(false);
110 }
111
112 public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i)
113 {
114 planeNormal = new Vector3();
115 planeSupport = new Vector3();
116 BulletDebug.Assert(false);
117 }
118
119 public override bool IsInside(Vector3 pt, float tolerance)
120 {
121 BulletDebug.Assert(false);
122 return false;
123 }
124
125 public override Vector3 LocalGetSupportingVertex(Vector3 vec)
126 {
127 Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec);
128
129 if (Margin != 0)
130 {
131 Vector3 vecnorm = vec;
132 if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))
133 {
134 vecnorm = new Vector3(-1, -1, -1);
135 }
136 vecnorm = Vector3.Normalize(vecnorm);
137 supVertex += Margin * vecnorm;
138 }
139 return supVertex;
140 }
141
142 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec0)
143 {
144 Vector3 supVec = new Vector3();
145
146 Vector3 vec = vec0;
147 float lenSqr = vec.LengthSquared();
148 if (lenSqr < 0.0001f)
149 {
150 vec = new Vector3(1, 0, 0);
151 }
152 else
153 {
154 float rlen = 1f / (float)Math.Sqrt(lenSqr);
155 vec *= rlen;
156 }
157
158 LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec);
159 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
160 _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax);
161 supVec = supportCallback.SupportVertexLocal;
162
163 return supVec;
164 }
165
166 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
167 {
168 //use 'w' component of supportVerticesOut?
169 /*{
170 for (int i = 0; i < numVectors; i++)
171 {
172 supportVerticesOut[i][3] = -1e30f;
173 }
174 }*/
175 for (int j = 0; j < vectors.Length; j++)
176 {
177 Vector3 vec = vectors[j];
178 LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(vec);
179 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
180 _stridingMesh.InternalProcessAllTriangles(supportCallback, -aabbMax, aabbMax);
181 supportVerticesOut[j] = supportCallback.SupportVertexLocal;
182 }
183 }
184 }
185}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs
new file mode 100644
index 0000000..9a86eaf
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShape.cs
@@ -0,0 +1,136 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// implements cylinder shape interface
31 /// </summary>
32 public class CylinderShape : BoxShape
33 {
34 public CylinderShape(Vector3 halfExtents)
35 : base(halfExtents)
36 {
37 }
38
39 public override BroadphaseNativeTypes ShapeType
40 {
41 get
42 {
43 return BroadphaseNativeTypes.Cylinder;
44 }
45 }
46
47 public virtual int UpAxis
48 {
49 get
50 {
51 return 1;
52 }
53 }
54
55 public virtual float Radius
56 {
57 get
58 {
59 return HalfExtents.Z;
60 }
61 }
62
63 //debugging
64 public override string Name
65 {
66 get
67 {
68 return "CylinderY";
69 }
70 }
71
72 //getAabb's default implementation is brute force, expected derived classes to implement a fast dedicated version
73 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
74 {
75 GetAabbSlow(t, out aabbMin, out aabbMax);
76 }
77
78 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
79 {
80 return CylinderLocalSupportY(HalfExtents, vec);
81 }
82
83 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
84 {
85 for (int i = 0; i < vectors.Length; i++)
86 {
87 supportVerticesOut[i] = CylinderLocalSupportY(HalfExtents, vectors[i]);
88 }
89 }
90
91 public override Vector3 LocalGetSupportingVertex(Vector3 vec)
92 {
93
94 Vector3 supVertex;
95 supVertex = LocalGetSupportingVertexWithoutMargin(vec);
96
97 if (Margin != 0)
98 {
99 Vector3 vecnorm = vec;
100 if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))
101 {
102 vecnorm=new Vector3(-1, -1, -1);
103 }
104 vecnorm = Vector3.Normalize(vecnorm);
105 supVertex += Margin * vecnorm;
106 }
107 return supVertex;
108 }
109
110 private Vector3 CylinderLocalSupportY(Vector3 halfExtents, Vector3 v)
111 {
112 float radius = halfExtents.X;
113 float halfHeight = halfExtents.Y;
114
115 Vector3 tmp = new Vector3();
116 float d;
117
118 float s = (float)Math.Sqrt(v.X * v.X + v.Z * v.Z);
119 if (s != 0)
120 {
121 d = radius / s;
122 tmp.X = v.X * d;
123 tmp.Y = v.Y < 0 ? -halfHeight : halfHeight;
124 tmp.Z = v.Z * d;
125 return tmp;
126 }
127 else
128 {
129 tmp.X = radius;
130 tmp.Y = v.Y < 0 ? -halfHeight : halfHeight;
131 tmp.Z = 0;
132 return tmp;
133 }
134 }
135 }
136}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs
new file mode 100644
index 0000000..9dc8e2d
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeX.cs
@@ -0,0 +1,100 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class CylinderShapeX : CylinderShape
30 {
31 public CylinderShapeX(Vector3 halfExtents)
32 : base(halfExtents) { }
33
34 public override int UpAxis
35 {
36 get
37 {
38 return 0;
39 }
40 }
41
42 public override float Radius
43 {
44 get
45 {
46 return HalfExtents.Y;
47 }
48 }
49
50 //debugging
51 public override string Name
52 {
53 get
54 {
55 return "CylinderX";
56 }
57 }
58
59 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
60 {
61 return CylinderLocalSupportX(HalfExtents, vec);
62 }
63
64 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
65 {
66 for (int i = 0; i < vectors.Length; i++)
67 {
68 supportVerticesOut[i] = CylinderLocalSupportX(HalfExtents, vectors[i]);
69 }
70 }
71
72 private Vector3 CylinderLocalSupportX(Vector3 halfExtents, Vector3 v)
73 {
74 //mapping depends on how cylinder local orientation is
75 // extents of the cylinder is: X,Y is for radius, and Z for height
76 float radius = halfExtents.Y;
77 float halfHeight = halfExtents.X;
78
79 Vector3 tmp = new Vector3();
80 float d;
81
82 float s = (float)Math.Sqrt(v.Y * v.Y + v.Z * v.Z);
83 if (s != 0)
84 {
85 d = radius / s;
86 tmp.Y = v.Y * d;
87 tmp.X = v.X < 0 ? -halfHeight : halfHeight;
88 tmp.Z = v.Z * d;
89 return tmp;
90 }
91 else
92 {
93 tmp.Y = radius;
94 tmp.X = v.X < 0 ? -halfHeight : halfHeight;
95 tmp.Z = 0;
96 return tmp;
97 }
98 }
99 }
100}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs
new file mode 100644
index 0000000..1913e9c
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/CylinderShapeZ.cs
@@ -0,0 +1,100 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class CylinderShapeZ : CylinderShape
30 {
31 public CylinderShapeZ(Vector3 halfExtents)
32 : base(halfExtents) { }
33
34 public override int UpAxis
35 {
36 get
37 {
38 return 2;
39 }
40 }
41
42 public override float Radius
43 {
44 get
45 {
46 return HalfExtents.X;
47 }
48 }
49
50 //debugging
51 public override string Name
52 {
53 get
54 {
55 return "CylinderZ";
56 }
57 }
58
59 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
60 {
61 return CylinderLocalSupportZ(HalfExtents, vec);
62 }
63
64 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
65 {
66 for (int i = 0; i < vectors.Length; i++)
67 {
68 supportVerticesOut[i] = CylinderLocalSupportZ(HalfExtents, vectors[i]);
69 }
70 }
71
72 Vector3 CylinderLocalSupportZ(Vector3 halfExtents, Vector3 v)
73 {
74 //mapping depends on how cylinder local orientation is
75 // extents of the cylinder is: X,Y is for radius, and Z for height
76 float radius = halfExtents.X;
77 float halfHeight = halfExtents.Z;
78
79 Vector3 tmp = new Vector3();
80 float d;
81
82 float s = (float)Math.Sqrt(v.X * v.X + v.Y * v.Y);
83 if (s != 0)
84 {
85 d = radius / s;
86 tmp.X = v.X * d;
87 tmp.Z = v.Z < 0 ? -halfHeight : halfHeight;
88 tmp.Y = v.Y * d;
89 return tmp;
90 }
91 else
92 {
93 tmp.X = radius;
94 tmp.Z = v.Z < 0 ? -halfHeight : halfHeight;
95 tmp.Y = 0;
96 return tmp;
97 }
98 }
99 }
100}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs
new file mode 100644
index 0000000..b094c39
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/EmptyShape.cs
@@ -0,0 +1,80 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26using System.Diagnostics;
27
28namespace XnaDevRu.BulletX
29{
30 public class EmptyShape : ConcaveShape
31 {
32 private Vector3 _localScaling;
33
34 public override string Name
35 {
36 get
37 {
38 return "Empty";
39 }
40 }
41
42 public override BroadphaseNativeTypes ShapeType
43 {
44 get
45 {
46 return BroadphaseNativeTypes.Empty;
47 }
48 }
49
50 public override Vector3 LocalScaling
51 {
52 get
53 {
54 return _localScaling;
55 }
56 set
57 {
58 _localScaling = value;
59 }
60 }
61
62 public override void ProcessAllTriangles(ITriangleCallback callback, MonoXnaCompactMaths.Vector3 aabbMin, MonoXnaCompactMaths.Vector3 aabbMax)
63 {
64 throw new Exception("The method or operation is not implemented.");
65 }
66
67 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
68 {
69 Vector3 margin = new Vector3(Margin, Margin, Margin);
70 aabbMin = t.Translation - margin;
71 aabbMax = t.Translation + margin;
72 }
73
74 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
75 {
76 inertia = new Vector3();
77 BulletDebug.Assert(false);
78 }
79 }
80}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs
new file mode 100644
index 0000000..5073096
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/FilteredCallback.cs
@@ -0,0 +1,55 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class FilteredCallback : ITriangleIndexCallback
30 {
31 private ITriangleCallback _callback;
32 private Vector3 _aabbMin;
33 private Vector3 _aabbMax;
34
35 public FilteredCallback(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
36 {
37 _callback = callback;
38 _aabbMin = aabbMin;
39 _aabbMax = aabbMax;
40 }
41
42 public ITriangleCallback TriangleCallback { get { return _callback; } set { _callback = value; } }
43 public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } }
44 public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } }
45
46 public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex)
47 {
48 if (MathHelper.TestTriangleAgainstAabb2(triangle, _aabbMin, _aabbMax))
49 {
50 //check aabb in triangle-space, before doing this
51 _callback.ProcessTriangle(triangle, partId, triangleIndex);
52 }
53 }
54 }
55}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs
new file mode 100644
index 0000000..b88d804
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/InternalTriangleIndexCallback.cs
@@ -0,0 +1,33 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public interface ITriangleIndexCallback
30 {
31 void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex);
32 }
33}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs
new file mode 100644
index 0000000..951707d
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/LocalSupportVertexCallback.cs
@@ -0,0 +1,58 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class LocalSupportVertexCallback : ITriangleIndexCallback
30 {
31 private Vector3 _supportVertexLocal;
32 private float _maxDot;
33 private Vector3 _supportVecLocal;
34
35 public LocalSupportVertexCallback(Vector3 supportVecLocal)
36 {
37 _supportVertexLocal = new Vector3();
38 _maxDot = -1e30f;
39 _supportVecLocal = supportVecLocal;
40 }
41
42 public float MaxDot { get { return _maxDot; } set { _maxDot = value; } }
43 public Vector3 SupportVertexLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } }
44
45 public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex)
46 {
47 for (int i = 0; i < 3; i++)
48 {
49 float dot = Vector3.Dot(_supportVecLocal, triangle[i]);
50 if (dot > _maxDot)
51 {
52 _maxDot = dot;
53 _supportVertexLocal = triangle[i];
54 }
55 }
56 }
57 }
58}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs
new file mode 100644
index 0000000..6f70cae
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MinkowskiSumShape.cs
@@ -0,0 +1,99 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// MinkowskiSumShape represents implicit (getSupportingVertex) based minkowski sum of two convex implicit shapes.
31 /// </summary>
32 public class MinkowskiSumShape : ConvexShape
33 {
34 private Matrix _transformA;
35 private Matrix _transformB;
36 private ConvexShape _shapeA;
37 private ConvexShape _shapeB;
38
39 public MinkowskiSumShape(ConvexShape shapeA, ConvexShape shapeB)
40 {
41 _shapeA = shapeA;
42 _shapeB = shapeB;
43 _transformA = Matrix.Identity;
44 _transformB = Matrix.Identity;
45 }
46
47 public Matrix TransformA { get { return _transformA; } set { _transformA = value; } }
48 public Matrix TransformB { get { return _transformB; } set { _transformB = value; } }
49 public ConvexShape ShapeA { get { return _shapeA; } }
50 public ConvexShape ShapeB { get { return _shapeB; } }
51
52 public override float Margin
53 {
54 get
55 {
56 return _shapeA.Margin + _shapeB.Margin;
57 }
58 set
59 {
60 base.Margin = value;
61 }
62 }
63
64 public override BroadphaseNativeTypes ShapeType
65 {
66 get
67 {
68 return BroadphaseNativeTypes.MinkowskiDifference;
69 }
70 }
71
72 public override string Name
73 {
74 get
75 {
76 return "MinkowskiSum";
77 }
78 }
79
80 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
81 {
82 Vector3 supVertexA = MathHelper.MatrixToVector(_transformA, _shapeA.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformA)));
83 Vector3 supVertexB = MathHelper.MatrixToVector(_transformB, _shapeB.LocalGetSupportingVertexWithoutMargin(Vector3.TransformNormal(vec, _transformB)));
84 return supVertexA + supVertexB;
85 }
86
87 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
88 {
89 for (int i = 0; i < vectors.Length; i++)
90 supportVerticesOut[i] = LocalGetSupportingVertexWithoutMargin(vectors[i]);
91 }
92
93 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
94 {
95 inertia = new Vector3();
96 BulletDebug.Assert(false);
97 }
98 }
99}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs
new file mode 100644
index 0000000..9959c01
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/MultiSphereShape.cs
@@ -0,0 +1,154 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// MultiSphereShape represents implicit convex hull of a collection of spheres (using getSupportingVertex)
31 /// </summary>
32 public class MultiSphereShape : ConvexShape
33 {
34 private const int _maxNumSpheres = 5;
35 private Vector3[] _localPositions = new Vector3[MaxNumSpheres];
36 private float[] _radi = new float[MaxNumSpheres];
37 private Vector3 _inertiaHalfExtents;
38
39 private int m_numSpheres;
40
41 public MultiSphereShape(Vector3 inertiaHalfExtents, Vector3[] positions, float[] radi, int numSpheres)
42 {
43 _inertiaHalfExtents = inertiaHalfExtents;
44 float startMargin = 1e30f;
45
46 m_numSpheres = numSpheres;
47 for (int i = 0; i < m_numSpheres; i++)
48 {
49 _localPositions[i] = positions[i];
50 _radi[i] = radi[i];
51 if (radi[i] < startMargin)
52 startMargin = radi[i];
53 }
54 Margin = startMargin;
55 }
56
57 public static int MaxNumSpheres { get { return _maxNumSpheres; } }
58
59 public override BroadphaseNativeTypes ShapeType
60 {
61 get
62 {
63 return BroadphaseNativeTypes.MultiSphere;
64 }
65 }
66
67 public override string Name
68 {
69 get
70 {
71 return "MultiSphere";
72 }
73 }
74
75 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vecA)
76 {
77 Vector3 supVec = new Vector3();
78
79 float maxDot = -1e30f;
80
81
82 Vector3 vec = vecA;
83 float lenSqr = vec.LengthSquared();
84 if (lenSqr < 0.0001f)
85 {
86 vec = new Vector3(1, 0, 0);
87 }
88 else
89 {
90 float rlen = 1f / (float)Math.Sqrt(lenSqr);
91 vec *= rlen;
92 }
93
94 Vector3 vtx;
95 float newDot;
96
97 for (int i = 0; i < m_numSpheres; i++)
98 {
99 vtx = _localPositions[i] + vec * LocalScaling * _radi[i] - vec * Margin;
100 newDot = Vector3.Dot(vec, vtx);
101 if (newDot > maxDot)
102 {
103 maxDot = newDot;
104 supVec = vtx;
105 }
106 }
107
108 return supVec;
109 }
110
111 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
112 {
113 for (int j = 0; j < vectors.Length; j++)
114 {
115 float maxDot = -1e30f;
116 Vector3 vtx;
117 float newDot;
118
119 for (int i = 0; i < m_numSpheres; i++)
120 {
121 vtx = _localPositions[i] + vectors[j] * LocalScaling * _radi[i] - vectors[j] * Margin;
122 newDot = Vector3.Dot(vectors[j], vtx);
123 if (newDot > maxDot)
124 {
125 maxDot = newDot;
126 supportVerticesOut[j] = vtx;
127 }
128 }
129 }
130 }
131
132 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
133 {
134 //as an approximation, take the inertia of the box that bounds the spheres
135 Matrix ident = Matrix.Identity;
136 Vector3 halfExtents = _inertiaHalfExtents;
137
138 float margin = ConvexDistanceMargin;
139
140 float lx = 2f * (halfExtents.X + margin);
141 float ly = 2f * (halfExtents.Y + margin);
142 float lz = 2f * (halfExtents.Z + margin);
143 float x2 = lx * lx;
144 float y2 = ly * ly;
145 float z2 = lz * lz;
146 float scaledmass = mass * 0.08333333f;
147
148 inertia = new Vector3();
149 inertia.X = scaledmass * (y2 + z2);
150 inertia.Y = scaledmass * (x2 + z2);
151 inertia.Z = scaledmass * (x2 + y2);
152 }
153 }
154}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs
new file mode 100644
index 0000000..fac344e
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/NodeOverlapCallback.cs
@@ -0,0 +1,32 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26namespace XnaDevRu.BulletX
27{
28 public interface INodeOverlapCallback
29 {
30 void ProcessNode(OptimizedBvhNode node);
31 }
32}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs
new file mode 100644
index 0000000..c22c990
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvh.cs
@@ -0,0 +1,293 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// OptimizedBvh store an AABB tree that can be quickly traversed on CPU (and SPU,GPU in future)
31 /// </summary>
32 public class OptimizedBvh
33 {
34 private static int _maxIterations = 0;
35 private OptimizedBvhNode _rootNode;
36
37 private OptimizedBvhNode[] _contiguousNodes;
38 private int _curNodeIndex;
39
40 private List<OptimizedBvhNode> _leafNodes = new List<OptimizedBvhNode>();
41
42 public OptimizedBvh() { }
43
44 public void Build(StridingMeshInterface triangles)
45 {
46 NodeTriangleCallback callback = new NodeTriangleCallback(_leafNodes);
47
48 Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
49 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
50
51 triangles.InternalProcessAllTriangles(callback, aabbMin, aabbMax);
52
53 //now we have an array of leafnodes in m_leafNodes
54 _contiguousNodes = new OptimizedBvhNode[2 * _leafNodes.Count];
55 for (int i = 0; i < _contiguousNodes.Length; i++)
56 _contiguousNodes[i] = new OptimizedBvhNode();
57 _curNodeIndex = 0;
58
59 _rootNode = BuildTree(_leafNodes, 0, _leafNodes.Count);
60 }
61
62 public OptimizedBvhNode BuildTree(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex)
63 {
64 OptimizedBvhNode internalNode;
65
66 int splitAxis, splitIndex, i;
67 int numIndices = endIndex - startIndex;
68 int curIndex = _curNodeIndex;
69
70 if (numIndices <= 0)
71 throw new BulletException();
72
73 if (numIndices == 1)
74 {
75 _contiguousNodes[_curNodeIndex++] = leafNodes[startIndex];
76 //return new (&m_contiguousNodes[m_curNodeIndex++]) btOptimizedBvhNode(leafNodes[startIndex]);
77 return leafNodes[startIndex];
78 }
79
80 //calculate Best Splitting Axis and where to split it. Sort the incoming 'leafNodes' array within range 'startIndex/endIndex'.
81 splitAxis = CalculateSplittingAxis(leafNodes, startIndex, endIndex);
82
83 splitIndex = SortAndCalculateSplittingIndex(leafNodes, startIndex, endIndex, splitAxis);
84
85 internalNode = _contiguousNodes[_curNodeIndex++];
86
87 internalNode.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f);
88 internalNode.AabbMin = new Vector3(1e30f, 1e30f, 1e30f);
89
90 for (i = startIndex; i < endIndex; i++)
91 {
92 internalNode.AabbMax = MathHelper.SetMax(internalNode.AabbMax, leafNodes[i].AabbMax);
93 internalNode.AabbMin = MathHelper.SetMin(internalNode.AabbMin, leafNodes[i].AabbMin);
94 }
95
96 //internalNode->m_escapeIndex;
97 internalNode.LeftChild = BuildTree(leafNodes, startIndex, splitIndex);
98 internalNode.RightChild = BuildTree(leafNodes, splitIndex, endIndex);
99
100 internalNode.EscapeIndex = _curNodeIndex - curIndex;
101 return internalNode;
102 }
103
104 public int CalculateSplittingAxis(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex)
105 {
106 Vector3 means = new Vector3();
107 Vector3 variance = new Vector3();
108 int numIndices = endIndex - startIndex;
109
110 for (int i = startIndex; i < endIndex; i++)
111 {
112 Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin);
113 means += center;
114 }
115 means *= (1f / (float)numIndices);
116
117 for (int i = startIndex; i < endIndex; i++)
118 {
119 Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin);
120 Vector3 diff2 = center - means;
121 diff2 = diff2 * diff2;
122 variance += diff2;
123 }
124 variance *= (1f / ((float)numIndices - 1));
125
126 return MathHelper.MaxAxis(variance);
127 }
128
129 public int SortAndCalculateSplittingIndex(List<OptimizedBvhNode> leafNodes, int startIndex, int endIndex, int splitAxis)
130 {
131 int splitIndex = startIndex;
132 int numIndices = endIndex - startIndex;
133 float splitValue;
134
135 Vector3 means = new Vector3();
136 for (int i = startIndex; i < endIndex; i++)
137 {
138 Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin);
139 means += center;
140 }
141 means *= (1f / (float)numIndices);
142
143 if (splitAxis == 0)
144 splitValue = means.X;
145 else if (splitAxis == 1)
146 splitValue = means.Y;
147 else if (splitAxis == 2)
148 splitValue = means.Z;
149 else
150 throw new ArgumentException();
151
152 //sort leafNodes so all values larger then splitValue comes first, and smaller values start from 'splitIndex'.
153 for (int i = startIndex; i < endIndex; i++)
154 {
155 Vector3 center = 0.5f * (leafNodes[i].AabbMax + leafNodes[i].AabbMin);
156 float centerSplit;
157
158 if (splitAxis == 0)
159 centerSplit = means.X;
160 else if (splitAxis == 1)
161 centerSplit = means.Y;
162 else if (splitAxis == 2)
163 centerSplit = means.Z;
164 else
165 throw new ArgumentException();
166
167 if (centerSplit > splitValue)
168 {
169 //swap
170 OptimizedBvhNode tmp = leafNodes[i];
171 leafNodes[i] = leafNodes[splitIndex];
172 leafNodes[splitIndex] = tmp;
173 splitIndex++;
174 }
175 }
176 if ((splitIndex == startIndex) || (splitIndex == (endIndex - 1)))
177 {
178 splitIndex = startIndex + (numIndices >> 1);
179 }
180 return splitIndex;
181 }
182
183 public void WalkTree(OptimizedBvhNode rootNode, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax)
184 {
185 bool isLeafNode, aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax);
186 if (aabbOverlap)
187 {
188 isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null);
189 if (isLeafNode)
190 {
191 nodeCallback.ProcessNode(rootNode);
192 }
193 else
194 {
195 WalkTree(rootNode.LeftChild, nodeCallback, aabbMin, aabbMax);
196 WalkTree(rootNode.RightChild, nodeCallback, aabbMin, aabbMax);
197 }
198 }
199 }
200
201 public void WalkStacklessTree(OptimizedBvhNode[] rootNodeArray, INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax)
202 {
203 int escapeIndex, curIndex = 0;
204 int walkIterations = 0;
205 bool aabbOverlap, isLeafNode;
206 int rootNodeIndex = 0;
207 OptimizedBvhNode rootNode = rootNodeArray[rootNodeIndex];
208
209 while (curIndex < _curNodeIndex)
210 {
211 //catch bugs in tree data
212 if (walkIterations >= _curNodeIndex)
213 throw new BulletException();
214
215 walkIterations++;
216 aabbOverlap = MathHelper.TestAabbAgainstAabb2(aabbMin, aabbMax, rootNode.AabbMin, rootNode.AabbMax);
217 isLeafNode = (rootNode.LeftChild == null && rootNode.RightChild == null);
218
219 if (isLeafNode && aabbOverlap)
220 {
221 nodeCallback.ProcessNode(rootNode);
222 }
223
224 if (aabbOverlap || isLeafNode)
225 {
226 rootNodeIndex++; // this
227 curIndex++;
228 if (rootNodeIndex < rootNodeArray.Length)
229 rootNode = rootNodeArray[rootNodeIndex];
230 }
231 else
232 {
233 escapeIndex = rootNode.EscapeIndex;
234 rootNodeIndex += escapeIndex; // and this
235 curIndex += escapeIndex;
236 if (rootNodeIndex < rootNodeArray.Length)
237 rootNode = rootNodeArray[rootNodeIndex];
238 }
239
240 }
241
242 if (_maxIterations < walkIterations)
243 _maxIterations = walkIterations;
244 }
245
246 public void ReportAabbOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax)
247 {
248 //either choose recursive traversal (walkTree) or stackless (walkStacklessTree)
249 //walkTree(m_rootNode1,nodeCallback,aabbMin,aabbMax);
250 //WalkStacklessTree(_rootNode, nodeCallback, aabbMin, aabbMax);
251 WalkStacklessTree(_contiguousNodes, nodeCallback, aabbMin, aabbMax);
252 }
253
254 public void ReportSphereOverlappingNodex(INodeOverlapCallback nodeCallback, Vector3 aabbMin, Vector3 aabbMax) { }
255 }
256
257 public class NodeTriangleCallback : ITriangleIndexCallback
258 {
259 private List<OptimizedBvhNode> _triangleNodes;
260
261 public NodeTriangleCallback(List<OptimizedBvhNode> triangleNodes)
262 {
263 _triangleNodes = triangleNodes;
264 }
265
266 public List<OptimizedBvhNode> TriangleNodes { get { return _triangleNodes; } set { _triangleNodes = value; } }
267
268 public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex)
269 {
270
271 OptimizedBvhNode node = new OptimizedBvhNode();
272 node.AabbMin = new Vector3(1e30f, 1e30f, 1e30f);
273 node.AabbMax = new Vector3(-1e30f, -1e30f, -1e30f);
274
275 node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[0]);
276 node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[0]);
277 node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[1]);
278 node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[1]);
279 node.AabbMin = MathHelper.SetMin(node.AabbMin, triangle[2]);
280 node.AabbMax = MathHelper.SetMax(node.AabbMax, triangle[2]);
281
282 node.EscapeIndex = -1;
283 node.LeftChild = null;
284 node.RightChild = null;
285
286 //for child nodes
287 node.SubPart = partId;
288 node.TriangleIndex = triangleIndex;
289
290 _triangleNodes.Add(node);
291 }
292 }
293}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs
new file mode 100644
index 0000000..d43f2fb
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/OptimizedBvhNode.cs
@@ -0,0 +1,63 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// OptimizedBvhNode contains both internal and leaf node information.
31 /// It hasn't been optimized yet for storage. Some obvious optimizations are:
32 /// Removal of the pointers (can already be done, they are not used for traversal)
33 /// and storing aabbmin/max as quantized integers.
34 /// 'subpart' doesn't need an integer either. It allows to re-use graphics triangle
35 /// meshes stored in a non-uniform way (like batches/subparts of triangle-fans
36 /// </summary>
37 public class OptimizedBvhNode
38 {
39 private Vector3 _aabbMin;
40 private Vector3 _aabbMax;
41
42 //these 2 pointers are obsolete, the stackless traversal just uses the escape index
43 private OptimizedBvhNode _leftChild;
44 private OptimizedBvhNode _rightChild;
45
46 private int _escapeIndex;
47
48 //for child nodes
49 private int _subPart;
50 private int _triangleIndex;
51
52 public Vector3 AabbMin { get { return _aabbMin; } set { _aabbMin = value; } }
53 public Vector3 AabbMax { get { return _aabbMax; } set { _aabbMax = value; } }
54
55 public OptimizedBvhNode LeftChild { get { return _leftChild; } set { _leftChild = value; } }
56 public OptimizedBvhNode RightChild { get { return _rightChild; } set { _rightChild = value; } }
57
58 public int EscapeIndex { get { return _escapeIndex; } set { _escapeIndex = value; } }
59
60 public int SubPart { get { return _subPart; } set { _subPart = value; } }
61 public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } }
62 }
63}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs
new file mode 100644
index 0000000..f61371a
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/PolyhedralConvexShape.cs
@@ -0,0 +1,133 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public abstract class PolyhedralConvexShape : ConvexShape
30 {
31 public PolyhedralConvexShape()
32 {
33 //m_optionalHull = null;
34 }
35
36 public abstract int VertexCount { get; }
37 public abstract int EdgeCount { get; }
38 public abstract int PlaneCount { get; }
39
40 public abstract void GetEdge(int i, out Vector3 pointA, out Vector3 pointB);
41 public abstract void GetVertex(int i, out Vector3 vertex);
42 public abstract void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i);
43 // abstract int getIndex(int i);
44
45 public abstract bool IsInside(Vector3 point, float tolerance);
46
47 // optional Hull is for optional Separating Axis Test Hull collision detection, see Hull.cpp
48 //public class Hull m_optionalHull;
49
50 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
51 {
52 Vector3 supVec = new Vector3();
53
54 float maxDot = -1e30f;
55
56 float lenSqr = vec.LengthSquared();
57 if (lenSqr < 0.0001f)
58 {
59 vec = new Vector3(1, 0, 0);
60 }
61 else
62 {
63 float rlen = 1f / (float)Math.Sqrt(lenSqr);
64 vec *= rlen;
65 }
66
67 Vector3 vtx;
68 float newDot;
69
70 for (int i = 0; i < VertexCount; i++)
71 {
72 GetVertex(i, out vtx);
73 newDot = Vector3.Dot(vec, vtx);
74 if (newDot > maxDot)
75 {
76 maxDot = newDot;
77 supVec = vtx;
78 }
79 }
80 return supVec;
81 }
82
83 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
84 {
85 #warning Think about this
86 /*Vector3 vtx;
87 float newDot;
88
89 for (int i = 0; i < vectors.Length; i++)
90 {
91 supportVerticesOut[i][3] = -1e30f;
92 }
93
94 for (int j = 0; j < vectors.Length; j++)
95 {
96 Vector3 vec = vectors[j];
97
98 for (int i = 0; i < getNumVertices(); i++)
99 {
100 getVertex(i, out vtx);
101 newDot = Vector3.Dot(vec,vtx);
102 if (newDot > supportVerticesOut[j][3])
103 {
104 //WARNING: don't swap next lines, the w component would get overwritten!
105 supportVerticesOut[j] = vtx;
106 supportVerticesOut[j][3] = newDot;
107 }
108 }
109 }*/
110 }
111
112 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
113 {
114 //not yet, return box inertia
115 float margin = Margin;
116
117 Matrix ident = Matrix.Identity;
118 Vector3 aabbMin, aabbMax;
119 GetAabb(ident, out aabbMin, out aabbMax);
120 Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f;
121
122 float lx = 2f * (halfExtents.X + margin);
123 float ly = 2f * (halfExtents.Y + margin);
124 float lz = 2f * (halfExtents.Z + margin);
125 float x2 = lx * lx;
126 float y2 = ly * ly;
127 float z2 = lz * lz;
128 float scaledmass = mass * 0.08333333f;
129
130 inertia = scaledmass * (new Vector3(y2 + z2, x2 + z2, x2 + y2));
131 }
132 }
133}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs
new file mode 100644
index 0000000..c660a6f
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SphereShape.cs
@@ -0,0 +1,116 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// btSphereShape implements an implicit (getSupportingVertex) Sphere
31 /// </summary>
32 public class SphereShape : ConvexShape
33 {
34 public SphereShape(float radius)
35 : base()
36 {
37 Vector3 temp = ImplicitShapeDimensions;
38 temp.X = radius;
39 ImplicitShapeDimensions = temp;
40 }
41
42 public float Radius { get { return ImplicitShapeDimensions.X; } }
43
44 public override BroadphaseNativeTypes ShapeType
45 {
46 get
47 {
48 return BroadphaseNativeTypes.Sphere;
49 }
50 }
51
52 public override string Name
53 {
54 get
55 {
56 return "Sphere";
57 }
58 }
59
60 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
61 {
62 return new Vector3();
63 }
64
65 /// <summary>
66 /// to improve gjk behaviour, use radius+margin as the full margin, so never get into the penetration case
67 /// this means, non-uniform scaling is not supported anymore
68 /// </summary>
69 public override float Margin
70 {
71 get
72 {
73 return LocalScaling.X * Radius + base.Margin;
74 }
75 set
76 {
77 base.Margin = value;
78 }
79 }
80
81 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
82 {
83 if (supportVerticesOut != null)
84 for (int i = 0; i < supportVerticesOut.Length; i++)
85 supportVerticesOut[i] = new Vector3();
86 }
87
88 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
89 {
90 float elem = 0.4f * mass * Margin * Margin;
91 inertia = new Vector3(elem, elem, elem);
92 }
93
94 public override Vector3 LocalGetSupportingVertex(Vector3 vec)
95 {
96 Vector3 supVertex = LocalGetSupportingVertexWithoutMargin(vec);
97
98 Vector3 vecnorm = vec;
99 if (vecnorm.LengthSquared() < (MathHelper.Epsilon * MathHelper.Epsilon))
100 {
101 vecnorm = new Vector3(-1f, -1f, -1f);
102 }
103 vecnorm.Normalize();
104 supVertex += Margin * vecnorm;
105 return supVertex;
106 }
107
108 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
109 {
110 Vector3 center = t.Translation;
111 Vector3 extent = new Vector3(Margin, Margin, Margin);
112 aabbMin = center - extent;
113 aabbMax = center + extent;
114 }
115 }
116}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs
new file mode 100644
index 0000000..49604a0
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StaticPlaneShape.cs
@@ -0,0 +1,124 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class StaticPlaneShape : ConcaveShape
30 {
31 private Vector3 _localAabbMin;
32 private Vector3 _localAabbMax;
33
34 private Vector3 _planeNormal;
35 private float _planeConstant;
36 private Vector3 _localScaling;
37
38 public StaticPlaneShape(Vector3 planeNormal, float planeConstant)
39 {
40 _planeNormal = planeNormal;
41 _planeConstant = planeConstant;
42 _localScaling = new Vector3();
43 }
44
45 protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } }
46 protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } }
47
48 protected Vector3 PlaneNormal { get { return _planeNormal; } set { _planeNormal = value; } }
49 protected float PlaneConstant { get { return _planeConstant; } set { _planeConstant = value; } }
50
51 public override BroadphaseNativeTypes ShapeType
52 {
53 get
54 {
55 return BroadphaseNativeTypes.StaticPlane;
56 }
57 }
58
59 public override Vector3 LocalScaling
60 {
61 get
62 {
63 return _localScaling;
64 }
65 set
66 {
67 _localScaling = value;
68 }
69 }
70
71 public override string Name
72 {
73 get
74 {
75 return "StaticPlane";
76 }
77 }
78
79 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
80 {
81 Vector3 infvec = new Vector3(1e30f, 1e30f, 1e30f);
82
83 Vector3 center = _planeNormal * _planeConstant;
84 aabbMin = center + infvec * _planeNormal;
85 aabbMax = aabbMin;
86 MathHelper.SetMin(ref aabbMin, center - infvec * _planeNormal);
87 MathHelper.SetMax(ref aabbMax, center - infvec * _planeNormal);
88
89 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
90 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
91 }
92
93 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
94 {
95 //moving concave objects not supported
96 inertia = new Vector3();
97 }
98
99 public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax) {
100 Vector3 halfExtents = (aabbMax - aabbMin) * 0.5f;
101 float radius = halfExtents.Length();
102 Vector3 center = (aabbMax + aabbMin) * 0.5f;
103
104 //this is where the triangles are generated, given AABB and plane equation (normal/constant)
105 Vector3 tangentDir0 = new Vector3(), tangentDir1 = new Vector3();
106
107 //tangentDir0/tangentDir1 can be precalculated
108 MathHelper.PlaneSpace1(_planeNormal, ref tangentDir0, ref tangentDir1);
109
110 Vector3 projectedCenter = center - (Vector3.Dot(_planeNormal, center) - _planeConstant) * _planeNormal;
111
112 Vector3[] triangle = new Vector3[3];
113 triangle[0] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius;
114 triangle[1] = projectedCenter + tangentDir0 * radius - tangentDir1 * radius;
115 triangle[2] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius;
116 callback.ProcessTriangle(triangle, 0, 0);
117
118 triangle[0] = projectedCenter - tangentDir0 * radius - tangentDir1 * radius;
119 triangle[1] = projectedCenter - tangentDir0 * radius + tangentDir1 * radius;
120 triangle[2] = projectedCenter + tangentDir0 * radius + tangentDir1 * radius;
121 callback.ProcessTriangle(triangle, 0, 1);
122 }
123 }
124}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs
new file mode 100644
index 0000000..2a2ce27
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/StridingMeshInterface.cs
@@ -0,0 +1,115 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// PHY_ScalarType enumerates possible scalar types.
31 /// See the StridingMeshInterface for its use
32 /// </summary>
33 public enum PHY_ScalarType
34 {
35 PHY_FLOAT,
36 PHY_DOUBLE,
37 PHY_INTEGER,
38 PHY_SHORT,
39 PHY_FIXEDPOINT88
40 }
41
42 /// <summary>
43 /// StridingMeshInterface is the interface class for high performance access to triangle meshes
44 /// It allows for sharing graphics and collision meshes. Also it provides locking/unlocking of graphics meshes that are in gpu memory.
45 /// </summary>
46 public abstract class StridingMeshInterface
47 {
48 protected Vector3 _scaling;
49
50 public StridingMeshInterface()
51 {
52 _scaling = new Vector3(1f,1f,1f);
53 }
54
55 public void InternalProcessAllTriangles(ITriangleIndexCallback callback, Vector3 aabbMin, Vector3 aabbMax)
56 {
57 int numtotalphysicsverts = 0;
58 int numtriangles, gfxindex;
59 int part, graphicssubparts = SubPartsCount();
60 Vector3[] triangle = new Vector3[3];
61 List<Vector3> verts;
62 List<int> indicies;
63
64 Vector3 meshScaling = Scaling;
65
66 //if the number of parts is big, the performance might drop due to the innerloop switch on indextype
67 for (part = 0; part < graphicssubparts; part++)
68 {
69 GetLockedReadOnlyVertexIndexBase(out verts, out indicies, out numtriangles, part);
70 numtotalphysicsverts += numtriangles * 3; //upper bound
71
72 for (gfxindex = 0; gfxindex < numtriangles; gfxindex++)
73 {
74 triangle[0] = verts[indicies[gfxindex * 3 + 0]];
75 triangle[1] = verts[indicies[gfxindex * 3 + 1]];
76 triangle[2] = verts[indicies[gfxindex * 3 + 2]];
77
78 callback.ProcessTriangleIndex(triangle, part, gfxindex);
79 }
80
81 UnLockReadOnlyVertexBase(part);
82 }
83 }
84
85
86 // get read and write access to a subpart of a triangle mesh
87 // this subpart has a continuous array of vertices and indices
88 // in this way the mesh can be handled as chunks of memory with striding
89 // very similar to OpenGL vertexarray support
90 // make a call to unLockVertexBase when the read and write access is finished
91 public abstract void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart);
92
93 public abstract void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart);
94
95 // unLockVertexBase finishes the access to a subpart of the triangle mesh
96 // make a call to unLockVertexBase when the read and write access (using getLockedVertexIndexBase) is finished
97 public abstract void UnLockVertexBase(int subpart);
98
99 public abstract void UnLockReadOnlyVertexBase(int subpart);
100
101
102 // getNumSubParts returns the number of seperate subparts
103 // each subpart has a continuous array of vertices and indices
104 public abstract int SubPartsCount();
105
106 public abstract void PreallocateVertices(int numverts);
107 public abstract void PreallocateIndices(int numindices);
108
109 public Vector3 Scaling
110 {
111 get { return _scaling; }
112 set { _scaling = value; }
113 }
114 }
115}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs
new file mode 100644
index 0000000..ffb5813
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/SupportVertexCallback.cs
@@ -0,0 +1,67 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 internal class SupportVertexCallback : ITriangleCallback
30 {
31 private Vector3 _supportVertexLocal;
32
33 private Matrix _worldTransform;
34 private float _maxDot;
35 private Vector3 _supportVecLocal;
36
37 public SupportVertexCallback(Vector3 supportVecWorld, Matrix trans)
38 {
39 _supportVertexLocal = new Vector3();
40 _worldTransform = trans;
41 _maxDot = -1e30f;
42 _supportVecLocal = Vector3.TransformNormal(supportVecWorld, _worldTransform);
43 }
44
45 public Matrix WorldTransform { get { return _worldTransform; } set { _worldTransform = value; } }
46 public float MaxDot { get { return _maxDot; } set { _maxDot = value; } }
47 public Vector3 SupportVectorLocal { get { return _supportVecLocal; } set { _supportVecLocal = value; } }
48
49 public Vector3 SupportVertexLocal { get { return _supportVertexLocal; } }
50 public Vector3 SupportVertexWorldSpace { get { return MathHelper.MatrixToVector(_worldTransform, _supportVertexLocal); } }
51
52 #region ITriangleCallback Members
53 public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex)
54 {
55 for (int i = 0; i < 3; i++)
56 {
57 float dot = Vector3.Dot(_supportVecLocal, triangle[i]);
58 if (dot > _maxDot)
59 {
60 _maxDot = dot;
61 _supportVertexLocal = triangle[i];
62 }
63 }
64 }
65 #endregion
66 }
67}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs
new file mode 100644
index 0000000..158e42a
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleBuffer.cs
@@ -0,0 +1,80 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class Triangle
30 {
31 private Vector3 _vertexA;
32 private Vector3 _vertexB;
33 private Vector3 _vertexC;
34 private int _partId;
35 private int _triangleIndex;
36
37 public Vector3 VertexA { get { return _vertexA; } set { _vertexA = value; } }
38 public Vector3 VertexB { get { return _vertexB; } set { _vertexB = value; } }
39 public Vector3 VertexC { get { return _vertexC; } set { _vertexC = value; } }
40 public int PartId { get { return _partId; } set { _partId = value; } }
41 public int TriangleIndex { get { return _triangleIndex; } set { _triangleIndex = value; } }
42 }
43
44 /// <summary>
45 /// example usage of this class:
46 /// TriangleBuffer triBuf;
47 /// concaveShape.processAllTriangles(triBuf, out aabbMin, out aabbMax);
48 /// for (int i = 0; i < triBuf.getNumTriangles(); i++)
49 /// {
50 /// Triangle tri = triBuf.getTriangle(i);
51 /// //do something useful here with the triangle
52 /// }
53 /// </summary>
54 public class TriangleBuffer : ITriangleCallback
55 {
56 private List<Triangle> _triangleBuffer = new List<Triangle>();
57
58 public int TriangleCount { get { return _triangleBuffer.Count; } }
59 public Triangle this[int index] { get { return _triangleBuffer[index]; } }
60
61 public void ClearBuffer()
62 {
63 _triangleBuffer.Clear();
64 }
65
66 #region ITriangleCallback Members
67 public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex)
68 {
69 Triangle tri = new Triangle();
70 tri.VertexA = triangle[0];
71 tri.VertexB = triangle[1];
72 tri.VertexC = triangle[2];
73 tri.PartId = partID;
74 tri.TriangleIndex = triangleIndex;
75
76 _triangleBuffer.Add(tri);
77 }
78 #endregion
79 }
80}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs
new file mode 100644
index 0000000..ac892c2
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleCallback.cs
@@ -0,0 +1,33 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public interface ITriangleCallback
30 {
31 void ProcessTriangle(Vector3[] triangle, int partId, int triangleIndex);
32 }
33}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs
new file mode 100644
index 0000000..6d0d7b5
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleIndexVertexArray.cs
@@ -0,0 +1,136 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// IndexedMesh indexes into existing vertex and index arrays, in a similar way OpenGL glDrawElements
31 /// instead of the number of indices, we pass the number of triangles
32 /// </summary>
33 public struct IndexedMesh
34 {
35 private int _numTriangles;
36 private int[] _triangleIndexBase;
37 private int _triangleIndexStride;
38 private int _numVertices;
39 private Vector3[] _vertexBase;
40 private int _vertexStride;
41
42 public IndexedMesh(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride)
43 {
44 _numTriangles = numTriangleIndices;
45 _triangleIndexBase = triangleIndexBase;
46 _triangleIndexStride = triangleIndexStride;
47 _vertexBase = vertexBase;
48 _numVertices = numVertices;
49 _vertexStride = vertexStride;
50 }
51
52 public IndexedMesh(int[] triangleIndexBase, Vector3[] vertexBase)
53 {
54 _numTriangles = triangleIndexBase.Length;
55 _triangleIndexBase = triangleIndexBase;
56 _triangleIndexStride = 32;
57 _vertexBase = vertexBase;
58 _numVertices = vertexBase.Length;
59 _vertexStride = 24;
60 }
61
62 public int TriangleCount { get { return _numTriangles; } set { _numTriangles = value; } }
63 public int[] TriangleIndexBase { get { return _triangleIndexBase; } set { _triangleIndexBase = value; } }
64 public int TriangleIndexStride { get { return _triangleIndexStride; } set { _triangleIndexStride = value; } }
65 public int VertexCount { get { return _numVertices; } set { _numVertices = value; } }
66 public Vector3[] VertexBase { get { return _vertexBase; } set { _vertexBase = value; } }
67 public int VertexStride { get { return _vertexStride; } set { _vertexStride = value; } }
68 }
69
70 /// <summary>
71 /// TriangleIndexVertexArray allows to use multiple meshes, by indexing into existing triangle/index arrays.
72 /// Additional meshes can be added using addIndexedMesh
73 /// </summary>
74 public class TriangleIndexVertexArray : StridingMeshInterface
75 {
76 List<IndexedMesh> _indexedMeshes = new List<IndexedMesh>();
77
78 public TriangleIndexVertexArray() { }
79
80 public TriangleIndexVertexArray(int numTriangleIndices, int[] triangleIndexBase, int triangleIndexStride, int numVertices, Vector3[] vertexBase, int vertexStride)
81 {
82 IndexedMesh mesh = new IndexedMesh();
83 mesh.TriangleCount = numTriangleIndices;
84 mesh.TriangleIndexBase = triangleIndexBase;
85 mesh.TriangleIndexStride = triangleIndexStride;
86 mesh.VertexBase = vertexBase;
87 mesh.VertexCount = numVertices;
88 mesh.VertexStride = vertexStride;
89
90 AddIndexedMesh(mesh);
91 }
92
93 public TriangleIndexVertexArray(int[] triangleIndexBase, Vector3[] vertexBase)
94 : this(triangleIndexBase.Length, triangleIndexBase, 32, vertexBase.Length, vertexBase, 24) { }
95
96 public void AddIndexedMesh(IndexedMesh indexedMesh)
97 {
98 _indexedMeshes.Add(indexedMesh);
99 }
100
101 public override void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart)
102 {
103 throw new Exception("The method or operation is not implemented.");
104 }
105
106 public override void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart)
107 {
108 throw new Exception("The method or operation is not implemented.");
109 }
110
111 public override void UnLockVertexBase(int subpart)
112 {
113 throw new Exception("The method or operation is not implemented.");
114 }
115
116 public override void UnLockReadOnlyVertexBase(int subpart)
117 {
118 throw new Exception("The method or operation is not implemented.");
119 }
120
121 public override int SubPartsCount()
122 {
123 return _indexedMeshes.Count;
124 }
125
126 public override void PreallocateVertices(int numverts)
127 {
128 throw new Exception("The method or operation is not implemented.");
129 }
130
131 public override void PreallocateIndices(int numindices)
132 {
133 throw new Exception("The method or operation is not implemented.");
134 }
135 }
136}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs
new file mode 100644
index 0000000..1426355
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMesh.cs
@@ -0,0 +1,102 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26using MonoXnaCompactMaths;
27
28namespace XnaDevRu.BulletX
29{
30 class TriangleMesh : StridingMeshInterface
31 {
32 int _numTriangles;
33 List<Vector3> _verts;
34
35 public TriangleMesh()
36 {
37 _numTriangles = 0;
38 _verts = new List<Vector3>();
39 }
40
41 void AddTriangle(Vector3 vertex0, Vector3 vertex1, Vector3 vertex2)
42 {
43 _verts.Add(vertex0);
44 _verts.Add(vertex1);
45 _verts.Add(vertex2);
46 _numTriangles++;
47 }
48
49 public override void GetLockedVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart)
50 {
51 verts = new List<Vector3>();
52 for (int i = 0; i < 3; i++)
53 {
54 verts.Add(_verts[subpart * 3 + i]);
55 }
56 indicies = new List<int>();
57 indicies.Add(0);
58 indicies.Add(1);
59 indicies.Add(2);
60 numfaces = 1;
61 }
62
63 public override void GetLockedReadOnlyVertexIndexBase(out List<Vector3> verts, out List<int> indicies, out int numfaces, int subpart)
64 {
65 verts = new List<Vector3>();
66 for (int i = 0; i < 3; i++)
67 {
68 verts.Add(_verts[subpart * 3 + i]);
69 }
70 indicies = new List<int>();
71 indicies.Add(0);
72 indicies.Add(1);
73 indicies.Add(2);
74 numfaces = 1;
75 }
76
77 public override void UnLockVertexBase(int subpart)
78 {
79
80 }
81
82 public override void UnLockReadOnlyVertexBase(int subpart)
83 {
84
85 }
86
87 public override int SubPartsCount()
88 {
89 return _numTriangles;
90 }
91
92 public override void PreallocateVertices(int numverts)
93 {
94
95 }
96
97 public override void PreallocateIndices(int numindices)
98 {
99
100 }
101 }
102}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs
new file mode 100644
index 0000000..bc8f09e
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleMeshShape.cs
@@ -0,0 +1,160 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// Concave triangle mesh. Uses an interface to access the triangles to allow for sharing graphics/physics triangles.
31 /// </summary>
32 public class TriangleMeshShape : ConcaveShape
33 {
34 private StridingMeshInterface _meshInterface;
35 private Vector3 _localAabbMin;
36 private Vector3 _localAabbMax;
37
38 public TriangleMeshShape(StridingMeshInterface meshInterface)
39 {
40 this._meshInterface = meshInterface;
41 RecalcLocalAabb();
42 }
43
44 protected StridingMeshInterface MeshInterface { get { return _meshInterface; } set { _meshInterface = value; } }
45 protected Vector3 LocalAabbMin { get { return _localAabbMin; } set { _localAabbMin = value; } }
46 protected Vector3 LocalAabbMax { get { return _localAabbMax; } set { _localAabbMax = value; } }
47
48 public override BroadphaseNativeTypes ShapeType
49 {
50 get
51 {
52 return BroadphaseNativeTypes.TriangleMesh;
53 }
54 }
55
56 public override Vector3 LocalScaling
57 {
58 get
59 {
60 return _meshInterface.Scaling;
61 }
62 set
63 {
64 _meshInterface.Scaling = value;
65 }
66 }
67
68 public override string Name
69 {
70 get
71 {
72 return "TriangleMesh";
73 }
74 }
75
76 public void RecalcLocalAabb()
77 {
78 {
79 Vector3 vec = new Vector3();
80 vec.X = 1f;
81 Vector3 tmp = LocalGetSupportingVertex(vec);
82 _localAabbMax.X = tmp.X + CollisionMargin;
83 vec.X = -1f;
84 tmp = LocalGetSupportingVertex(vec);
85 _localAabbMin.X = tmp.X - CollisionMargin;
86 }
87 {
88 Vector3 vec = new Vector3();
89 vec.Y = 1f;
90 Vector3 tmp = LocalGetSupportingVertex(vec);
91 _localAabbMax.Y = tmp.Y + CollisionMargin;
92 vec.Y = -1f;
93 tmp = LocalGetSupportingVertex(vec);
94 _localAabbMin.Y = tmp.Y - CollisionMargin;
95 }
96 {
97 Vector3 vec = new Vector3();
98 vec.Z = 1f;
99 Vector3 tmp = LocalGetSupportingVertex(vec);
100 _localAabbMax.Z = tmp.Z + CollisionMargin;
101 vec.Z = -1f;
102 tmp = LocalGetSupportingVertex(vec);
103 _localAabbMin.Z = tmp.Z - CollisionMargin;
104 }
105 }
106
107 public override void ProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
108 {
109 LocalProcessAllTriangles(callback, aabbMax, aabbMax);
110 }
111
112 protected void LocalProcessAllTriangles(ITriangleCallback callback, Vector3 aabbMin, Vector3 aabbMax)
113 {
114 FilteredCallback filterCallback = new FilteredCallback(callback, aabbMin, aabbMax);
115 _meshInterface.InternalProcessAllTriangles(filterCallback, aabbMin, aabbMax);
116 }
117
118 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
119 {
120 Vector3 localHalfExtents = 0.5f * (_localAabbMax - _localAabbMin);
121 Vector3 localCenter = 0.5f * (_localAabbMax + _localAabbMin);
122
123 Matrix abs_b = MathHelper.Absolute(t);
124
125 Vector3 center = MathHelper.MatrixToVector(t, localCenter);
126
127 Vector3 extent = new Vector3(Vector3.Dot(new Vector3(abs_b.M11, abs_b.M12, abs_b.M13), localHalfExtents),
128 Vector3.Dot(new Vector3(abs_b.M21, abs_b.M22, abs_b.M23), localHalfExtents),
129 Vector3.Dot(new Vector3(abs_b.M31, abs_b.M32, abs_b.M33), localHalfExtents));
130 extent += new Vector3(Margin, Margin, Margin);
131
132 aabbMin = center - extent;
133 aabbMax = center + extent;
134 }
135
136 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
137 {
138 inertia = new Vector3();
139 //moving concave objects not supported
140 BulletDebug.Assert(false);
141 }
142
143 public virtual Vector3 LocalGetSupportingVertex(Vector3 vec)
144 {
145 Vector3 supportVertex;
146 Matrix ident = Matrix.Identity;
147 SupportVertexCallback supportCallback = new SupportVertexCallback(vec, ident);
148 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
149 LocalProcessAllTriangles(supportCallback, -aabbMax, aabbMax);
150 supportVertex = supportCallback.SupportVertexLocal;
151 return supportVertex;
152 }
153
154 public virtual Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
155 {
156 BulletDebug.Assert(false);
157 return LocalGetSupportingVertex(vec);
158 }
159 }
160}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs
new file mode 100644
index 0000000..59ffad0
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/CollisionShapes/TriangleShape.cs
@@ -0,0 +1,187 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class TriangleShape : PolyhedralConvexShape
30 {
31 private Vector3[] _vertices = new Vector3[3];
32
33 public TriangleShape(Vector3 pointA, Vector3 pointB, Vector3 pointC)
34 {
35 _vertices[0] = pointA;
36 _vertices[1] = pointB;
37 _vertices[2] = pointC;
38 }
39
40 public override int PreferredPenetrationDirectionsCount
41 {
42 get
43 {
44 return 2;
45 }
46 }
47
48 public Vector3[] Vertices
49 {
50 get
51 {
52 return _vertices;
53 }
54 }
55
56 public override int VertexCount
57 {
58 get
59 {
60 return 3;
61 }
62 }
63
64 public override int EdgeCount
65 {
66 get
67 {
68 return 3;
69 }
70 }
71
72 public override int PlaneCount
73 {
74 get
75 {
76 return 1;
77 }
78 }
79
80 public override BroadphaseNativeTypes ShapeType
81 {
82 get
83 {
84 return BroadphaseNativeTypes.Triangle;
85 }
86 }
87
88 public override string Name
89 {
90 get
91 {
92 return "Triangle";
93 }
94 }
95
96 public override void GetPreferredPenetrationDirection(int index, out Vector3 penetrationVector)
97 {
98 CalculateNormal(out penetrationVector);
99 if (index != 0)
100 penetrationVector *= -1f;
101 }
102
103 public virtual void GetPlaneEquation(int i, out Vector3 planeNormal, out Vector3 planeSupport)
104 {
105 CalculateNormal(out planeNormal);
106 planeSupport = _vertices[0];
107 }
108
109 public void CalculateNormal(out Vector3 normal)
110 {
111 normal = Vector3.Normalize(Vector3.Cross(_vertices[1] - _vertices[0], _vertices[2] - _vertices[0]));
112 }
113
114 public override Vector3 LocalGetSupportingVertexWithoutMargin(Vector3 vec)
115 {
116 Vector3 dots = new Vector3(Vector3.Dot(vec, _vertices[0]), Vector3.Dot(vec, _vertices[1]), Vector3.Dot(vec, _vertices[2]));
117 return _vertices[MathHelper.MaxAxis(dots)];
118 }
119
120 public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(Vector3[] vectors, Vector3[] supportVerticesOut)
121 {
122 for (int i = 0; i < vectors.Length; i++)
123 {
124 Vector3 dir = vectors[i];
125 Vector3 dots = new Vector3(Vector3.Dot(dir, _vertices[0]), Vector3.Dot(dir, _vertices[1]), Vector3.Dot(dir, _vertices[2]));
126 supportVerticesOut[i] = _vertices[MathHelper.MaxAxis(dots)];
127 }
128 }
129
130 public override void CalculateLocalInertia(float mass, out Vector3 inertia)
131 {
132 inertia = new Vector3();
133 BulletDebug.Assert(false);
134 }
135
136 public override void GetEdge(int i, out Vector3 pa, out Vector3 pb)
137 {
138 GetVertex(i, out pa);
139 GetVertex((i + 1) % 3, out pb);
140 }
141
142 public override void GetAabb(Matrix t, out Vector3 aabbMin, out Vector3 aabbMax)
143 {
144 GetAabbSlow(t, out aabbMin, out aabbMax);
145 }
146
147 public override void GetVertex(int i, out Vector3 vtx)
148 {
149 vtx = _vertices[i];
150 }
151
152 public override void GetPlane(out Vector3 planeNormal, out Vector3 planeSupport, int i)
153 {
154 GetPlaneEquation(i, out planeNormal, out planeSupport);
155 }
156
157 public override bool IsInside(Vector3 pt, float tolerance)
158 {
159 Vector3 normal;
160 CalculateNormal(out normal);
161 //distance to plane
162 float dist = Vector3.Dot(pt, normal);
163 float planeconst = Vector3.Dot(_vertices[0], normal);
164 dist -= planeconst;
165 if (dist >= -tolerance && dist <= tolerance)
166 {
167 //inside check on edge-planes
168 int i;
169 for (i = 0; i < 3; i++)
170 {
171 Vector3 pa, pb;
172 GetEdge(i, out pa, out pb);
173 Vector3 edge = pb - pa;
174 Vector3 edgeNormal = Vector3.Cross(edge, normal);
175 edgeNormal = Vector3.Normalize(edgeNormal);
176 float distance = Vector3.Dot(pt, edgeNormal);
177 float edgeConst = Vector3.Dot(pa, edgeNormal);
178 distance -= edgeConst;
179 if (distance < -tolerance)
180 return false;
181 }
182 return true;
183 }
184 return false;
185 }
186 }
187}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs
new file mode 100644
index 0000000..497f220
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ContinuousConvexCollision.cs
@@ -0,0 +1,199 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// ContinuousConvexCollision implements angular and linear time of impact for convex objects.
31 /// Based on Brian Mirtich's Conservative Advancement idea (PhD thesis).
32 /// Algorithm operates in worldspace, in order to keep inbetween motion globally consistent.
33 /// It uses GJK at the moment. Future improvement would use minkowski sum / supporting vertex, merging innerloops
34 /// </summary>
35 public class ContinuousConvexCollision : IConvexCast
36 {
37 /// <summary>
38 /// This maximum should not be necessary. It allows for untested/degenerate cases in production code.
39 /// You don't want your game ever to lock-up.
40 /// </summary>
41 private const int MaxIterations = 1000;
42
43 private ISimplexSolver _simplexSolver;
44 private IConvexPenetrationDepthSolver _penetrationDepthSolver;
45 private ConvexShape _convexA;
46 private ConvexShape _convexB;
47
48 public ContinuousConvexCollision(ConvexShape convexA, ConvexShape convexB,
49 ISimplexSolver simplexSolver, IConvexPenetrationDepthSolver penetrationDepthSolver)
50 {
51 _simplexSolver = simplexSolver;
52 _penetrationDepthSolver = penetrationDepthSolver;
53 _convexA = convexA;
54 _convexB = convexB;
55 }
56
57 public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
58 {
59 _simplexSolver.Reset();
60
61 // compute linear and angular velocity for this interval, to interpolate
62 Vector3 linVelA = new Vector3(), angVelA = new Vector3(), linVelB = new Vector3(), angVelB = new Vector3();
63 TransformUtil.CalculateVelocity(fromA, toA, 1f, ref linVelA, ref angVelA);
64 TransformUtil.CalculateVelocity(fromB, toB, 1f, ref linVelB, ref angVelB);
65
66 float boundingRadiusA = _convexA.GetAngularMotionDisc();
67 float boundingRadiusB = _convexB.GetAngularMotionDisc();
68
69 float maxAngularProjectedVelocity = angVelA.Length() * boundingRadiusA +
70 angVelB.Length() * boundingRadiusB;
71
72 float radius = 0.001f;
73
74 float lambda = 0f;
75 Vector3 v = new Vector3(1f, 0f, 0f);
76
77 int maxIter = MaxIterations;
78
79 Vector3 n = new Vector3();
80 bool hasResult = false;
81 Vector3 c;
82
83 float lastLambda = lambda;
84 //float epsilon = 0.001f;
85
86 int numIter = 0;
87 //first solution, using GJK
88
89
90 Matrix identityTrans = Matrix.Identity;
91
92 SphereShape raySphere = new SphereShape(0f);
93 raySphere.Margin=0f;
94
95
96 //result.drawCoordSystem(sphereTr);
97
98 PointCollector pointCollector1 = new PointCollector();
99
100 GjkPairDetector gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
101 GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
102
103 //we don't use margins during CCD
104 gjk.setIgnoreMargin(true);
105
106 input.TransformA = fromA;
107 input.TransformB = fromB;
108
109 DiscreteCollisionDetectorInterface.Result r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
110 gjk.GetClosestPoints(input, r, null);
111
112 hasResult = pointCollector1.HasResult;
113 c = pointCollector1.PointInWorld;
114
115 if (hasResult)
116 {
117 float dist;
118 dist = pointCollector1.Distance;
119 n = pointCollector1.NormalOnBInWorld;
120
121 //not close enough
122 while (dist > radius)
123 {
124 numIter++;
125 if (numIter > maxIter)
126 return false; //todo: report a failure
127
128 float dLambda = 0f;
129
130 //calculate safe moving fraction from distance / (linear+rotational velocity)
131
132 //float clippedDist = GEN_min(angularConservativeRadius,dist);
133 //float clippedDist = dist;
134
135 float projectedLinearVelocity = Vector3.Dot(linVelB - linVelA, n);
136
137 dLambda = dist / (projectedLinearVelocity + maxAngularProjectedVelocity);
138
139 lambda = lambda + dLambda;
140
141 if (lambda > 1f) return false;
142 if (lambda < 0f) return false;
143
144 //todo: next check with relative epsilon
145 if (lambda <= lastLambda)
146 break;
147 lastLambda = lambda;
148
149
150 //interpolate to next lambda
151 Matrix interpolatedTransA = new Matrix(), interpolatedTransB = new Matrix(), relativeTrans;
152
153 TransformUtil.IntegrateTransform(fromA, linVelA, angVelA, lambda, ref interpolatedTransA);
154 TransformUtil.IntegrateTransform(fromB, linVelB, angVelB, lambda, ref interpolatedTransB);
155
156 relativeTrans = MathHelper.InverseTimes(interpolatedTransB, interpolatedTransA);
157
158 result.DebugDraw(lambda);
159
160 PointCollector pointCollector = new PointCollector();
161 gjk = new GjkPairDetector(_convexA, _convexB, (VoronoiSimplexSolver)_simplexSolver, _penetrationDepthSolver);
162 input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
163 input.TransformA = interpolatedTransA;
164 input.TransformB = interpolatedTransB;
165
166 // !!!!!!!!!!
167 r = (DiscreteCollisionDetectorInterface.Result)pointCollector1;
168 gjk.GetClosestPoints(input, r, null);
169
170 if (pointCollector.HasResult)
171 {
172 if (pointCollector.Distance < 0f)
173 {
174 //degenerate ?!
175 result.Fraction = lastLambda;
176 result.Normal = n;
177 return true;
178 }
179 c = pointCollector.PointInWorld;
180
181 dist = pointCollector.Distance;
182 }
183 else
184 {
185 //??
186 return false;
187 }
188
189 }
190
191 result.Fraction = lambda;
192 result.Normal = n;
193 return true;
194 }
195
196 return false;
197 }
198 }
199}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs
new file mode 100644
index 0000000..9e88a7e
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ConvexCast.cs
@@ -0,0 +1,73 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29
30 /// <summary>
31 /// CastResult stores the closest result
32 /// alternatively, add a callback method to decide about closest/all results
33 /// </summary>
34 public class CastResult
35 {
36 private Vector3 _normal;
37 private float _fraction;
38 private Matrix _hitTransformA;
39 private Matrix _hitTransformB;
40 private IDebugDraw _debugDrawer;
41
42 public CastResult()
43 {
44 _fraction = 1e30f;
45 }
46
47 public Vector3 Normal { get { return _normal; } set { _normal = value; } }
48 public float Fraction { get { return _fraction; } set { _fraction = value; } }
49 public Matrix HitTransformA { get { return _hitTransformA; } set { _hitTransformA = value; } }
50 public Matrix HitTransformB { get { return _hitTransformB; } set { _hitTransformB = value; } }
51 public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } }
52
53 public virtual void DebugDraw(float fraction) { }
54 public virtual void DrawCoordSystem(Matrix trans) { }
55 }
56
57 /// <summary>
58 /// ConvexCast is an interface for Casting
59 /// </summary>
60 public interface IConvexCast
61 {
62 /// <summary>
63 /// cast a convex against another convex object
64 /// </summary>
65 /// <param name="fromA"></param>
66 /// <param name="toA"></param>
67 /// <param name="fromB"></param>
68 /// <param name="toB"></param>
69 /// <param name="result"></param>
70 /// <returns></returns>
71 bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result);
72 }
73}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs
new file mode 100644
index 0000000..fae2557
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/DiscreteCollisionDetectorInterface.cs
@@ -0,0 +1,117 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public abstract class DiscreteCollisionDetectorInterface
30 {
31 public abstract class Result
32 {
33 public abstract void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB);
34 public abstract void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth);
35 }
36
37 public class ClosestPointInput
38 {
39 private float _maximumDistanceSquared;
40 private Matrix _transformA, _transformB;
41
42 #region Properties
43 public Matrix TransformB
44 {
45 get { return _transformB; }
46 set { _transformB = value; }
47 }
48
49 public Matrix TransformA
50 {
51 get { return _transformA; }
52 set { _transformA = value; }
53 }
54
55 public float MaximumDistanceSquared
56 {
57 get { return _maximumDistanceSquared; }
58 set { _maximumDistanceSquared = value; }
59 }
60 #endregion
61
62 public ClosestPointInput()
63 {
64 _maximumDistanceSquared = 1e30f;
65 }
66 }
67
68 public abstract void GetClosestPoints(ClosestPointInput input, Result output, IDebugDraw debugDraw);
69 }
70
71 public class StorageResult : DiscreteCollisionDetectorInterface.Result
72 {
73 private Vector3 _closestPointInB;
74 private Vector3 _normalOnSurfaceB;
75 private float _distance; //negative means penetration !
76
77 #region Properties
78
79 public float Distance
80 {
81 get { return _distance; }
82 set { _distance = value; }
83 }
84 public Vector3 NormalOnSurfaceB
85 {
86 get { return _normalOnSurfaceB; }
87 set { _normalOnSurfaceB = value; }
88 }
89 public Vector3 ClosestPointInB
90 {
91 get { return _closestPointInB; }
92 set { _closestPointInB = value; }
93 }
94
95 #endregion
96
97 public StorageResult()
98 {
99 _distance = 1e30f;
100 }
101
102 public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth)
103 {
104 if (depth < _distance)
105 {
106 _normalOnSurfaceB = normalOnBInWorld;
107 _closestPointInB = pointInWorld;
108 _distance = depth;
109 }
110 }
111
112 public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1)
113 {
114 throw new Exception("The method or operation is not implemented.");
115 }
116 }
117}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs
new file mode 100644
index 0000000..4ee9599
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkConvexCast.cs
@@ -0,0 +1,176 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// GjkConvexCast performs a raycast on a convex object using support mapping.
31 /// </summary>
32 public class GjkConvexCast : IConvexCast
33 {
34 private VoronoiSimplexSolver _simplexSolver;
35 private ConvexShape _convexA, _convexB;
36
37 public GjkConvexCast(ConvexShape convexShapeA, ConvexShape convexShapeB, VoronoiSimplexSolver solver)
38 {
39 _simplexSolver = solver;
40
41 _convexA = convexShapeA;
42 _convexB = convexShapeB;
43 }
44
45 #region IConvexCast Members
46
47 /// <summary>
48 /// cast a convex against another convex object
49 /// </summary>
50 /// <param name="fromA"></param>
51 /// <param name="toA"></param>
52 /// <param name="fromB"></param>
53 /// <param name="toB"></param>
54 /// <param name="result"></param>
55 /// <returns></returns>
56 public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
57 {
58 MinkowskiSumShape combined = new MinkowskiSumShape(_convexA, _convexB);
59
60 Matrix rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;
61 Matrix rayToLocalA = MathHelper.InvertMatrix(toA) * toB;
62
63 Matrix transformA = fromA;
64 Matrix transformB = fromB;
65
66 transformA.Translation = new Vector3(0, 0, 0);
67 transformB.Translation = new Vector3(0, 0, 0);
68
69 combined.TransformA = transformA;
70 combined.TransformB = transformB;
71
72 float radius = 0.01f;
73 float lambda = 0;
74
75 Vector3 s = rayFromLocalA.Translation;
76 Vector3 r = rayToLocalA.Translation - rayFromLocalA.Translation;
77 Vector3 x = s;
78 Vector3 n = new Vector3();
79 Vector3 c = new Vector3();
80
81 bool hasResult = false;
82 float lastLambda = lambda;
83
84 IConvexPenetrationDepthSolver penSolver = null;
85 Matrix identityTransform = Matrix.Identity;
86
87 SphereShape raySphere = new SphereShape(0.0f);
88 raySphere.Margin=0.0f;
89
90 Matrix sphereTransform = Matrix.Identity;
91 sphereTransform.Translation = rayFromLocalA.Translation;
92
93 result.DrawCoordSystem(sphereTransform);
94
95 {
96 PointCollector pointCollector = new PointCollector();
97 GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
98
99 GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
100 input.TransformA = sphereTransform;
101 input.TransformB = identityTransform;
102
103 gjk.GetClosestPoints(input, pointCollector, null);
104
105 hasResult = pointCollector.HasResult;
106
107 c = pointCollector.PointInWorld;
108 n = pointCollector.NormalOnBInWorld;
109 }
110
111 if (hasResult)
112 {
113 float dist = (c - x).Length();
114
115 if (dist < radius)
116 {
117 lastLambda = 1.0f;
118 }
119
120 while (dist > radius)
121 {
122 n = x - c;
123 float dot = Vector3.Dot(n, r);
124
125 if (dot >= -(MathHelper.Epsilon * MathHelper.Epsilon)) return false;
126
127 lambda = lambda - Vector3.Distance(n, n) / dot;
128 if (lambda <= lastLambda) break;
129
130 lastLambda = lambda;
131
132 x = s + lambda * r;
133
134 sphereTransform.Translation = x;
135 result.DrawCoordSystem(sphereTransform);
136 PointCollector pointCollector = new PointCollector();
137
138 GjkPairDetector gjk = new GjkPairDetector(raySphere, combined, _simplexSolver, penSolver);
139 GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
140 input.TransformA = sphereTransform;
141 input.TransformB = identityTransform;
142
143 gjk.GetClosestPoints(input, pointCollector, null);
144
145 if (pointCollector.HasResult)
146 {
147 if (pointCollector.Distance < 0.0f)
148 {
149 result.Fraction = lastLambda;
150 result.Normal = n;
151 return true;
152 }
153
154 c = pointCollector.PointInWorld;
155 dist = (c - x).Length();
156 }
157 else
158 {
159 return false;
160 }
161 }
162
163 if (lastLambda < 1.0f)
164 {
165 result.Fraction = lastLambda;
166 result.Normal = n;
167 return true;
168 }
169 }
170
171 return false;
172 }
173
174 #endregion
175 }
176}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs
new file mode 100644
index 0000000..50ae7ab
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpa.cs
@@ -0,0 +1,633 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// GJK-EPA collision solver by Nathanael Presson
31 /// Nov.2006
32 /// </summary>
33 public class GjkEpa
34 {
35 //private static readonly int _precision = 1 /* U(sizeof(F) == 4)*/;
36
37 private static readonly float _infinity = MathHelper.Infinity;
38 //private static readonly float _pi = (float)Math.PI;
39 private static readonly float _twoPi = (float)(Math.PI * 2);
40
41 private static readonly int _gjkMaxIterations = 128;
42 private static readonly int _gjkHashSize = 1 << 6;
43 private static readonly int _gjkHashMask = _gjkHashSize - 1;
44 private static readonly float _gjkInSimplexEpsilon = 0.0001f;
45 private static readonly float _gjkSquareInSimplexEpsilon = _gjkInSimplexEpsilon * _gjkInSimplexEpsilon;
46
47 private static readonly int _epaMaxIterations = 256;
48 private static readonly float _epaInFaceEpsilon = 0.01f;
49 private static readonly float _epaAccuracy = 0.001f;
50
51 public static float EpaAccuracy { get { return _epaAccuracy; } }
52
53 private static float Abs(float v) { return (v < 0 ? -v : v); }
54 private static float Sign(float v) { return (v < 0 ? -1 : 1); }
55
56 static void Swap<T>(ref T a, ref T b)
57 {
58 T t = a;
59 a = b;
60 b = t;
61 }
62
63 public class Gjk
64 {
65 public class MinkowskiVertice
66 {
67 private Vector3 _vertice; /* Minkowski vertice */
68 private Vector3 _ray; /* Ray */
69
70 public Vector3 Vertice { get { return _vertice; } set { _vertice = value; } }
71 public Vector3 Ray { get { return _ray; } set { _ray = value; } }
72 }
73 public class He
74 {
75 private Vector3 _ray;
76 private He _next;
77
78 public He Next { get { return _next; } set { _next = value; } }
79 public Vector3 Ray { get { return _ray; } set { _ray = value; } }
80 }
81
82 private He[] _table = new He[_gjkHashSize];
83 private Matrix[] _wrotations = new Matrix[2];
84 private Vector3[] _positions = new Vector3[2];
85 private ConvexShape[] _shapes = new ConvexShape[2];
86 private MinkowskiVertice[] _simplex = new MinkowskiVertice[5];
87 private Vector3 _ray;
88 private int _order;
89 private int _iterations;
90 private float _margin;
91 private bool _failed;
92
93 public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA,
94 Matrix wrotationB, Vector3 positionB, ConvexShape shapeB)
95 : this(wrotationA, positionA, shapeA, wrotationB, positionB, shapeB, 0) { }
96
97 public Gjk(Matrix wrotationA, Vector3 positionA, ConvexShape shapeA,
98 Matrix wrotationB, Vector3 positionB, ConvexShape shapeB,
99 float pmargin)
100 {
101 for (int i = 0; i < _simplex.Length; i++)
102 _simplex[i] = new MinkowskiVertice();
103
104 for (int i = 0; i < _wrotations.Length; i++)
105 _wrotations[i] = new Matrix();
106
107 for (int i = 0; i < _positions.Length; i++)
108 _positions[i] = new Vector3();
109
110 _wrotations[0] = wrotationA; _positions[0] = positionA;
111 _shapes[0] = shapeA;
112 _wrotations[0].Translation = Vector3.Zero;
113 _wrotations[1] = wrotationB; _positions[1] = positionB;
114 _shapes[1] = shapeB;
115 _wrotations[1].Translation = Vector3.Zero;
116 //sablock = sa->BeginBlock();
117 _margin = pmargin;
118 _failed = false;
119 }
120
121 public bool Failed { get { return _failed; } }
122 public int Iterations { get { return _iterations; } }
123 public int Order { get { return _order; } }
124 public MinkowskiVertice[] Simplex { get { return _simplex; } }
125
126 public int Hash(Vector3 v)
127 {
128 int h = ((int)(v.X * 15461) ^ (int)(v.Y * 83003) ^ (int)(v.Z * 15473));
129 return (h * 169639) & _gjkHashMask;
130 }
131
132 public bool FetchSupport()
133 {
134 int h = Hash(_ray);
135 He e = _table[h];
136 while (e != null)
137 {
138 if (e.Ray == _ray)
139 {
140 --_order;
141 return (false);
142 }
143 else e = e.Next;
144 }
145 e = new He();
146 e.Ray = _ray;
147 e.Next = _table[h];
148 _table[h] = e;
149 Support(_ray, ref _simplex[++_order]);
150 return (Vector3.Dot(_ray, _simplex[_order].Vertice) > 0);
151 }
152
153 public Vector3 LocalSupport(Vector3 d, int i)
154 {
155 Matrix m = _wrotations[i];
156 m.Translation = Vector3.Zero;
157 Vector3 vtx = Vector3.TransformNormal(d, m);
158 Vector3 result = MathHelper.MatrixToVector(_wrotations[i], _shapes[i].LocalGetSupportingVertex(vtx));
159 return (result + _positions[i]);
160 }
161
162 public void Support(Vector3 d, ref MinkowskiVertice v)
163 {
164 v.Ray = d;
165 v.Vertice = LocalSupport(d, 0) - LocalSupport(-d, 1) + d * _margin;
166 }
167
168 public bool SolveSimplex2(Vector3 ao, Vector3 ab)
169 {
170 if (Vector3.Dot(ab, ao) >= 0)
171 {
172 Vector3 cabo = Vector3.Cross(ab, ao);
173 if (cabo.LengthSquared() > _gjkSquareInSimplexEpsilon)
174 { _ray = Vector3.Cross(cabo, ab); }
175 else
176 { return true; }
177 }
178 else
179 {
180 _order = 0;
181 _simplex[0].Ray = _simplex[1].Ray;
182 _simplex[0].Vertice = _simplex[1].Vertice;
183
184 _ray = ao;
185 }
186 return false;
187 }
188
189 public bool SolveSimplex3(Vector3 ao, Vector3 ab, Vector3 ac)
190 {
191 return (SolveSimplex3a(ao, ab, ac, Vector3.Cross(ab, ac)));
192 }
193
194 public bool SolveSimplex3a(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 cabc)
195 {
196 if ((Vector3.Dot(Vector3.Cross(cabc, ab), ao)) < -_gjkInSimplexEpsilon)
197 {
198 _order = 1;
199 _simplex[0].Vertice = _simplex[1].Vertice;
200 _simplex[0].Ray = _simplex[1].Ray;
201
202 _simplex[1].Vertice = _simplex[2].Vertice;
203 _simplex[1].Ray = _simplex[2].Ray;
204
205 return (SolveSimplex2(ao, ab));
206 }
207 else if (Vector3.Dot(Vector3.Cross(cabc, ac), ao) > +_gjkInSimplexEpsilon)
208 {
209 _order = 1;
210 _simplex[1].Vertice = _simplex[2].Vertice;
211 _simplex[1].Ray = _simplex[2].Ray;
212
213 return (SolveSimplex2(ao, ac));
214 }
215 else
216 {
217 float d = Vector3.Dot(cabc, ao);
218 if (Abs(d) > _gjkInSimplexEpsilon)
219 {
220 if (d > 0)
221 { _ray = cabc; }
222 else
223 { _ray = -cabc; Swap<MinkowskiVertice>(ref _simplex[0], ref _simplex[1]); }
224 return (false);
225 }
226 else return (true);
227 }
228 }
229
230 public bool SolveSimplex4(Vector3 ao, Vector3 ab, Vector3 ac, Vector3 ad)
231 {
232 Vector3 crs;
233 if (Vector3.Dot((crs = Vector3.Cross(ab, ac)), ao) > _gjkInSimplexEpsilon)
234 {
235 _order = 2;
236 _simplex[0].Vertice = _simplex[1].Vertice;
237 _simplex[0].Ray = _simplex[1].Ray;
238
239 _simplex[1].Vertice = _simplex[2].Vertice;
240 _simplex[1].Ray = _simplex[2].Ray;
241
242 _simplex[2].Vertice = _simplex[3].Vertice;
243 _simplex[2].Ray = _simplex[3].Ray;
244
245 return (SolveSimplex3a(ao, ab, ac, crs));
246 }
247 else if (Vector3.Dot((crs = Vector3.Cross(ac, ad)), ao) > _gjkInSimplexEpsilon)
248 {
249 _order = 2;
250 _simplex[2].Vertice = _simplex[3].Vertice;
251 _simplex[2].Ray = _simplex[3].Ray;
252
253 return (SolveSimplex3a(ao, ac, ad, crs));
254 }
255 else if (Vector3.Dot((crs = Vector3.Cross(ad, ab)), ao) > _gjkInSimplexEpsilon)
256 {
257 _order = 2;
258
259 _simplex[1].Vertice = _simplex[0].Vertice;
260 _simplex[1].Ray = _simplex[0].Ray;
261
262 _simplex[0].Vertice = _simplex[2].Vertice;
263 _simplex[0].Ray = _simplex[2].Ray;
264
265 _simplex[2].Vertice = _simplex[3].Vertice;
266 _simplex[2].Ray = _simplex[3].Ray;
267
268 return (SolveSimplex3a(ao, ad, ab, crs));
269 }
270 else return (true);
271 }
272
273 public bool SearchOrigin()
274 {
275 return SearchOrigin(new Vector3(1, 0, 0));
276 }
277
278 public bool SearchOrigin(Vector3 initray)
279 {
280 _iterations = 0;
281 unchecked
282 {
283 _order = (int)(-1);
284 }
285 _failed = false;
286 _ray = Vector3.Normalize(initray);
287
288 //ClearMemory(table, sizeof(void*) * GJK_hashsize);
289 for (int i = 0; i < _table.Length; i++)
290 _table[i] = null;
291 FetchSupport();
292 _ray = -_simplex[0].Vertice;
293 for (; _iterations < _gjkMaxIterations; ++_iterations)
294 {
295 float rl = _ray.Length();
296 _ray /= rl > 0 ? rl : 1;
297 if (FetchSupport())
298 {
299 bool found = (false);
300 switch (_order)
301 {
302 case 1: found = SolveSimplex2(-_simplex[1].Vertice, _simplex[0].Vertice - _simplex[1].Vertice); break;
303 case 2: found = SolveSimplex3(-_simplex[2].Vertice, _simplex[1].Vertice - _simplex[2].Vertice, _simplex[0].Vertice - _simplex[2].Vertice); break;
304 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;
305 }
306 if (found) return (true);
307 }
308 else return (false);
309 }
310 _failed = true;
311 return (false);
312 }
313
314 public bool EncloseOrigin()
315 {
316 switch (_order)
317 {
318 /* Point */
319 case 0: break;
320 /* Line */
321 case 1:
322 Vector3 ab = _simplex[1].Vertice - _simplex[0].Vertice;
323 Vector3[] b ={ Vector3.Cross(ab, new Vector3(1, 0, 0)),
324 Vector3.Cross(ab, new Vector3(0, 1, 0)),
325 Vector3.Cross(ab, new Vector3(0, 0, 1)) };
326 float[] m ={ b[0].LengthSquared(), b[1].LengthSquared(), b[2].LengthSquared() };
327 Matrix r = Matrix.CreateFromQuaternion(new Quaternion(Vector3.Normalize(ab), _twoPi / 3));
328 Vector3 w = b[m[0] > m[1] ? m[0] > m[2] ? 0 : 2 : m[1] > m[2] ? 1 : 2];
329 Support(Vector3.Normalize(w), ref _simplex[4]); w = Vector3.TransformNormal(w, r);
330 Support(Vector3.Normalize(w), ref _simplex[2]); w = Vector3.TransformNormal(w, r);
331 Support(Vector3.Normalize(w), ref _simplex[3]); w = Vector3.TransformNormal(w, r);
332 _order = 4;
333 return true;
334 /* Triangle */
335 case 2:
336 Vector3 n = Vector3.Normalize(Vector3.Cross(_simplex[1].Vertice - _simplex[0].Vertice, _simplex[2].Vertice - _simplex[0].Vertice));
337 Support(n, ref _simplex[3]);
338 Support(-n, ref _simplex[4]);
339 _order = 4;
340 return true;
341 /* Tetrahedron */
342 case 3: return (true);
343 /* Hexahedron */
344 case 4: return (true);
345 }
346 return (false);
347 }
348 }
349
350 public class Epa
351 {
352 public class Face
353 {
354 public Gjk.MinkowskiVertice[] _vertices = new Gjk.MinkowskiVertice[3];
355 public Face[] _faces = new Face[3];
356 public int[] _e = new int[3];
357 public Vector3 _n;
358 public float _d;
359 public int _mark;
360 public Face _prev;
361 public Face _next;
362 }
363
364 private Gjk _gjk;
365 private Face _root;
366 private int _nfaces;
367 private int _iterations;
368 private Vector3[,] _features = new Vector3[2, 3];
369 private Vector3[] _nearest = new Vector3[2];
370 private Vector3 _normal;
371 private float _depth;
372 private bool _failed;
373
374 public Epa(Gjk gjk)
375 {
376 this._gjk = gjk;
377 }
378
379 public bool Failed { get { return _failed; } }
380 public int Iterations { get { return _iterations; } }
381 public Vector3 Normal { get { return _normal; } }
382 public Vector3[] Nearest { get { return _nearest; } }
383
384 public Vector3 GetCoordinates(Face face)
385 {
386 Vector3 o = face._n * -face._d;
387 float[] a ={ Vector3.Cross(face._vertices[0].Vertice - o, face._vertices[1].Vertice - o).Length(),
388 Vector3.Cross(face._vertices[1].Vertice - o, face._vertices[2].Vertice - o).Length(),
389 Vector3.Cross(face._vertices[2].Vertice - o, face._vertices[0].Vertice - o).Length()};
390 float sm = a[0] + a[1] + a[2];
391 return (new Vector3(a[1], a[2], a[0]) / (sm > 0 ? sm : 1));
392 }
393
394 public Face FindBest()
395 {
396 Face bf = null;
397 if (_root != null)
398 {
399 Face cf = _root;
400 float bd = _infinity;
401 do
402 {
403 if (cf._d < bd) { bd = cf._d; bf = cf; }
404 } while (null != (cf = cf._next));
405 }
406 return bf;
407 }
408
409 public bool Set(ref Face f, Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c)
410 {
411 Vector3 nrm = Vector3.Cross(b.Vertice - a.Vertice, c.Vertice - a.Vertice);
412 float len = nrm.Length();
413 bool valid = (Vector3.Dot(Vector3.Cross(a.Vertice, b.Vertice), nrm) >= -_epaInFaceEpsilon &&
414 Vector3.Dot(Vector3.Cross(b.Vertice, c.Vertice), nrm) >= -_epaInFaceEpsilon &&
415 Vector3.Dot(Vector3.Cross(c.Vertice, a.Vertice), nrm) >= -_epaInFaceEpsilon);
416 f._vertices[0] = a;
417 f._vertices[1] = b;
418 f._vertices[2] = c;
419 f._mark = 0;
420 f._n = nrm / (len > 0 ? len : _infinity);
421 f._d = Max(0, -Vector3.Dot(f._n, a.Vertice));
422 return valid;
423 }
424
425 public Face NewFace(Gjk.MinkowskiVertice a, Gjk.MinkowskiVertice b, Gjk.MinkowskiVertice c)
426 {
427 Face pf = new Face();
428 if (Set(ref pf, a, b, c))
429 {
430 if (_root != null) _root._prev = pf;
431 pf._prev = null;
432 pf._next = _root;
433 _root = pf;
434 ++_nfaces;
435 }
436 else
437 {
438 pf._prev = pf._next = null;
439 }
440 return (pf);
441 }
442
443 public void Detach(ref Face face)
444 {
445 if (face._prev != null || face._next != null)
446 {
447 --_nfaces;
448 if (face == _root)
449 {
450 _root = face._next;
451 _root._prev = null;
452 }
453 else
454 {
455 if (face._next == null)
456 {
457 face._prev._next = null;
458 }
459 else
460 {
461 face._prev._next = face._next;
462 face._next._prev = face._prev;
463 }
464 }
465 face._prev = face._next = null;
466 }
467 }
468
469 public void Link(ref Face f0, int e0, ref Face f1, int e1)
470 {
471 f0._faces[e0] = f1; f1._e[e1] = e0;
472 f1._faces[e1] = f0; f0._e[e0] = e1;
473 }
474
475 public Gjk.MinkowskiVertice Support(Vector3 w)
476 {
477 Gjk.MinkowskiVertice v = new Gjk.MinkowskiVertice();
478 _gjk.Support(w, ref v);
479 return v;
480 }
481
482 private static int[] mod3 ={ 0, 1, 2, 0, 1 };
483
484 public int BuildHorizon(int markid, Gjk.MinkowskiVertice w, ref Face f, int e, ref Face cf, ref Face ff)
485 {
486 int ne = (0);
487 if (f._mark != markid)
488 {
489 int e1 = (mod3[e + 1]);
490 if ((Vector3.Dot(f._n, w.Vertice) + f._d) > 0)
491 {
492 Face nf = NewFace(f._vertices[e1], f._vertices[e], w);
493 Link(ref nf, 0, ref f, e);
494 if (cf != null) Link(ref cf, 1, ref nf, 2); else ff = nf;
495 cf = nf; ne = 1;
496 }
497 else
498 {
499 int e2 = (mod3[e + 2]);
500 Detach(ref f);
501 f._mark = markid;
502 ne += BuildHorizon(markid, w, ref f._faces[e1], f._e[e1], ref cf, ref ff);
503 ne += BuildHorizon(markid, w, ref f._faces[e2], f._e[e2], ref cf, ref ff);
504 }
505 }
506 return (ne);
507 }
508
509 public float EvaluatePD()
510 {
511 return EvaluatePD(_epaAccuracy);
512 }
513
514 private int[,] fidx;
515 private int[,] eidx;
516
517 public float EvaluatePD(float accuracy)
518 {
519 //Block* sablock = sa->BeginBlock();
520 Face bestface = null;
521 int markid = 1;
522 _depth = -_infinity;
523 _normal = new Vector3();
524 _root = null;
525 _nfaces = 0;
526 _iterations = 0;
527 _failed = false;
528 /* Prepare hull */
529 if (_gjk.EncloseOrigin())
530 {
531 int nfidx = 0;
532 int neidx = 0;
533 Gjk.MinkowskiVertice[] basemkv = new Gjk.MinkowskiVertice[5];
534 Face[] basefaces = new Face[6];
535 switch (_gjk.Order)
536 {
537 /* Tetrahedron */
538 case 3:
539 {
540 fidx = new int[,] { { 2, 1, 0 }, { 3, 0, 1 }, { 3, 1, 2 }, { 3, 2, 0 } };
541 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 } };
542 nfidx = 4; neidx = 6;
543 } break;
544 /* Hexahedron */
545 case 4:
546 {
547 fidx = new int[,] { { 2, 0, 4 }, { 4, 1, 2 }, { 1, 4, 0 }, { 0, 3, 1 }, { 0, 2, 3 }, { 1, 3, 2 } };
548 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 } };
549 nfidx = 6; neidx = 9;
550 } break;
551 }
552 int i;
553
554 for (i = 0; i <= _gjk.Order; ++i)
555 {
556 //basemkv[i] = (GJK::Mkv*)sa->Allocate(sizeof(GJK::Mkv));
557 basemkv[i] = new Gjk.MinkowskiVertice();
558 basemkv[i].Vertice = _gjk.Simplex[i].Vertice;
559 basemkv[i].Ray = _gjk.Simplex[i].Ray;
560 }
561 for (i = 0; i < nfidx; ++i)
562 {
563 basefaces[i] = NewFace(basemkv[fidx[i, 0]], basemkv[fidx[i, 1]], basemkv[fidx[i, 2]]);
564 }
565 for (i = 0; i < neidx; ++i)
566 {
567 Link(ref basefaces[eidx[i, 0]], eidx[i, 1], ref basefaces[eidx[i, 2]], eidx[i, 3]);
568 }
569 }
570 if (0 == _nfaces)
571 {
572 return _depth;
573 }
574 /* Expand hull */
575 for (; _iterations < _epaMaxIterations; ++_iterations)
576 {
577 Face bf = FindBest();
578 if (bf != null)
579 {
580 Gjk.MinkowskiVertice w = Support(-bf._n);
581 float d = Vector3.Dot(bf._n, w.Vertice) + bf._d;
582 bestface = bf;
583 if (d < -accuracy)
584 {
585 Face cf = null;
586 Face ff = null;
587 int nf = 0;
588 Detach(ref bf);
589 bf._mark = ++markid;
590 for (int i = 0; i < 3; ++i)
591 {
592 nf += BuildHorizon(markid, w, ref bf._faces[i], bf._e[i], ref cf, ref ff);
593 }
594 if (nf <= 2) { break; }
595 Link(ref cf, 1, ref ff, 2);
596 }
597 else break;
598 }
599 else break;
600 }
601 /* Extract contact */
602 if (bestface != null)
603 {
604 Vector3 b = GetCoordinates(bestface);
605 _normal = bestface._n;
606 _depth = Max(0, bestface._d);
607 for (int i = 0; i < 2; ++i)
608 {
609 float s = i != 0 ? -1 : 1;
610 for (int j = 0; j < 3; ++j)
611 {
612 _features[i, j] = _gjk.LocalSupport(s * bestface._vertices[j].Ray, i);
613 }
614 }
615 _nearest[0] = _features[0, 0] * b.X + _features[0, 1] * b.Y + _features[0, 2] * b.Z;
616 _nearest[1] = _features[1, 0] * b.X + _features[1, 1] * b.Y + _features[1, 2] * b.Z;
617 }
618 else _failed = true;
619 return _depth;
620 }
621
622 private float Max(float a, float b)
623 {
624 return (a > b ? a : b);
625 }
626
627 private float Min(float a, float b)
628 {
629 return (a < b ? a : b);
630 }
631 }
632 }
633}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs
new file mode 100644
index 0000000..739b4fc
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaPenetrationDepthSolver.cs
@@ -0,0 +1,56 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// EpaPenetrationDepthSolver uses the Expanding Polytope Algorithm to
31 /// calculate the penetration depth between two convex shapes.
32 /// </summary>
33 public class GjkEpaPenetrationDepthSolver : IConvexPenetrationDepthSolver
34 {
35 public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver, ConvexShape convexA, ConvexShape convexB, Matrix transformA, Matrix transformB, Vector3 vector, out Vector3 ptrA, out Vector3 ptrB, IDebugDraw debugDraw)
36 {
37 float radialmargin = 0;
38
39 GjkEpaSolver.Results results;
40 if (GjkEpaSolver.Collide(convexA, transformA,
41 convexB, transformB,
42 radialmargin, out results))
43 {
44 // debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
45 //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
46 ptrA = results.Witnesses[0];
47 ptrB = results.Witnesses[1];
48 return true;
49 }
50 ptrA = new Vector3();
51 ptrB = new Vector3();
52
53 return false;
54 }
55 }
56}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs
new file mode 100644
index 0000000..aa9d61e
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkEpaSolver.cs
@@ -0,0 +1,101 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// GjkEpaSolver contributed under zlib by Nathanael Presson
31 /// </summary>
32 public class GjkEpaSolver
33 {
34 public struct Results
35 {
36 public enum Status
37 {
38 Separated, /* Shapes doesnt penetrate */
39 Penetrating, /* Shapes are penetrating */
40 GjkFailed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */
41 EpaFailed, /* EPA phase fail, bigger problem, need to save parameters, and debug */
42 }
43
44 private Vector3[] _witnesses;
45 private Vector3 _normal;
46 private float _depth;
47 private int _epaIterations;
48 private int _gjkIterations;
49 private Status _status;
50
51 public Vector3[] Witnesses { get { return _witnesses; } set { _witnesses = value; } }
52 public Vector3 Normal { get { return _normal; } set { _normal = value; } }
53 public float Depth { get { return _depth; } set { _depth = value; } }
54 public int EpaIterations { get { return _epaIterations; } set { _epaIterations = value; } }
55 public int GjkIterations { get { return _gjkIterations; } set { _gjkIterations = value; } }
56 public Status ResultStatus { get { return _status; } set { _status = value; } }
57 }
58
59 public static bool Collide(ConvexShape shapeA, Matrix wtrsA,
60 ConvexShape shapeB, Matrix wtrsB,
61 float radialmargin,
62 out Results results)
63 {
64 /* Initialize */
65 results = new Results();
66 results.Witnesses = new Vector3[2];
67 results.Witnesses[0] =
68 results.Witnesses[1] =
69 results.Normal = new Vector3();
70 results.Depth = 0;
71 results.ResultStatus = Results.Status.Separated;
72 results.EpaIterations = 0;
73 results.GjkIterations = 0;
74 /* Use GJK to locate origin */
75 GjkEpa.Gjk gjk = new GjkEpa.Gjk(wtrsA, wtrsA.Translation, shapeA,
76 wtrsB, wtrsB.Translation, shapeB,
77 radialmargin + GjkEpa.EpaAccuracy);
78 bool collide = gjk.SearchOrigin();
79 results.GjkIterations = gjk.Iterations + 1;
80 if (collide)
81 {
82 /* Then EPA for penetration depth */
83 GjkEpa.Epa epa = new GjkEpa.Epa(gjk);
84 float pd = epa.EvaluatePD();
85 results.EpaIterations = epa.Iterations + 1;
86 if (pd > 0)
87 {
88 results.ResultStatus = Results.Status.Penetrating;
89 results.Normal = epa.Normal;
90 results.Depth = pd;
91 results.Witnesses[0] = epa.Nearest[0];
92 results.Witnesses[1] = epa.Nearest[1];
93 return true;
94 }
95 else { if (epa.Failed) results.ResultStatus = Results.Status.EpaFailed; }
96 }
97 else { if (gjk.Failed) results.ResultStatus = Results.Status.GjkFailed; }
98 return false;
99 }
100 }
101}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs
new file mode 100644
index 0000000..0831ff5
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/GjkPairDetector.cs
@@ -0,0 +1,343 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class GjkPairDetector : DiscreteCollisionDetectorInterface
30 {
31 private Vector3 _cachedSeparatingAxis;
32 private IConvexPenetrationDepthSolver _penetrationDepthSolver;
33 private ISimplexSolver _simplexSolver;
34 private ConvexShape _minkowskiA, _minkowskiB;
35 private bool _ignoreMargin;
36
37 private int _lastUsedMethod;
38 private int _currentIteration;
39 private int _degenerateSimplex;
40 private int _catchDegeneracies;
41
42 private static int _numDeepPenetrationChecks = 0;
43 private static int _numGjkChecks = 0;
44
45 private const float RelativeError2 = 1.0e-6f;
46
47 #region Properties
48 public int LastUsedMethod
49 {
50 get { return _lastUsedMethod; }
51 set { _lastUsedMethod = value; }
52 }
53
54 public int CurrentIteration
55 {
56 get { return _currentIteration; }
57 set { _currentIteration = value; }
58 }
59
60 public int DegenerateSimplex
61 {
62 get { return _degenerateSimplex; }
63 set { _degenerateSimplex = value; }
64 }
65
66 public int CatchDegeneracies
67 {
68 get { return _catchDegeneracies; }
69 set { _catchDegeneracies = value; }
70 }
71
72 public static int DeepPenetrationChecksCount { get { return _numDeepPenetrationChecks; } }
73 public static int GjkChecksCount { get { return _numGjkChecks; } }
74 #endregion
75
76 public GjkPairDetector(ConvexShape objectA, ConvexShape objectB,
77 ISimplexSolver simplexSolver,
78 IConvexPenetrationDepthSolver penetrationDepthSolver)
79 {
80 _cachedSeparatingAxis = new Vector3(0, 0, 1);
81
82 _penetrationDepthSolver = penetrationDepthSolver;
83 _simplexSolver = simplexSolver;
84 _minkowskiA = objectA;
85 _minkowskiB = objectB;
86 _ignoreMargin = false;
87 _lastUsedMethod = -1;
88 _catchDegeneracies = 1;
89 }
90
91 public void setMinkowskiA(ConvexShape minkA)
92 {
93 _minkowskiA = minkA;
94 }
95
96 public void setMinkowskiB(ConvexShape minkB)
97 {
98 _minkowskiB = minkB;
99 }
100 public void setCachedSeperatingAxis(Vector3 seperatingAxis)
101 {
102 _cachedSeparatingAxis = seperatingAxis;
103 }
104
105 public void setPenetrationDepthSolver(IConvexPenetrationDepthSolver penetrationDepthSolver)
106 {
107 this._penetrationDepthSolver = penetrationDepthSolver;
108 }
109
110 public void setIgnoreMargin(bool ignoreMargin)
111 {
112 this._ignoreMargin = ignoreMargin;
113 }
114
115 public override void GetClosestPoints(DiscreteCollisionDetectorInterface.ClosestPointInput input, DiscreteCollisionDetectorInterface.Result output, IDebugDraw debugDraw)
116 {
117 float distance = 0;
118
119 Vector3 normalInB = new Vector3();
120 Vector3 pointOnA = new Vector3(), pointOnB = new Vector3();
121
122 Matrix localTransA = input.TransformA;
123 Matrix localTransB = input.TransformB;
124
125 Vector3 positionOffset = (localTransA.Translation + localTransB.Translation) * 0.5f;
126 localTransA.Translation -= positionOffset;
127 localTransB.Translation -= positionOffset;
128
129 float marginA = _minkowskiA.Margin;
130 float marginB = _minkowskiB.Margin;
131
132 _numGjkChecks++;
133
134 if (_ignoreMargin)
135 {
136 marginA = 0;
137 marginB = 0;
138 }
139
140 _currentIteration = 0;
141
142 int gjkMaxIter = 1000;
143 _cachedSeparatingAxis = new Vector3(0, 1, 0);
144
145 bool isValid = false;
146 bool checkSimplex = false;
147 bool checkPenetration = true;
148 _degenerateSimplex = 0;
149
150 _lastUsedMethod = -1;
151
152 {
153 float squaredDistance = MathHelper.Infinity;
154 float delta = 0;
155
156 float margin = marginA + marginB;
157
158 _simplexSolver.Reset();
159
160 while (true)
161 {
162 Matrix transABasis = input.TransformA;
163 transABasis.Translation = Vector3.Zero;
164
165 Matrix transBBasis = input.TransformB;
166 transBBasis.Translation = Vector3.Zero;
167
168 Vector3 seperatingAxisInA = Vector3.TransformNormal(-_cachedSeparatingAxis, transABasis);
169 Vector3 seperatingAxisInB = Vector3.TransformNormal(_cachedSeparatingAxis, transBBasis);
170
171 Vector3 pInA = _minkowskiA.LocalGetSupportingVertexWithoutMargin(seperatingAxisInA);
172 Vector3 qInB = _minkowskiB.LocalGetSupportingVertexWithoutMargin(seperatingAxisInB);
173 Vector3 pWorld = MathHelper.MatrixToVector(localTransA, pInA);
174 Vector3 qWorld = MathHelper.MatrixToVector(localTransB, qInB);
175
176 Vector3 w = pWorld - qWorld;
177 delta = Vector3.Dot(_cachedSeparatingAxis, w);
178
179 if ((delta > 0.0) && (delta * delta > squaredDistance * input.MaximumDistanceSquared))
180 {
181 checkPenetration = false;
182 break;
183 }
184
185 if (_simplexSolver.InSimplex(w))
186 {
187 _degenerateSimplex = 1;
188 checkSimplex = true;
189 break;
190 }
191
192 float f0 = squaredDistance - delta;
193 float f1 = squaredDistance * RelativeError2;
194
195 if (f0 <= f1)
196 {
197 if (f0 <= 0.0f)
198 {
199 _degenerateSimplex = 2;
200 }
201
202 checkSimplex = true;
203 break;
204 }
205
206 _simplexSolver.AddVertex(w, pWorld, qWorld);
207
208 if (!_simplexSolver.Closest(out _cachedSeparatingAxis))
209 {
210 _degenerateSimplex = 3;
211 checkSimplex = true;
212 break;
213 }
214
215 float previouseSquaredDistance = squaredDistance;
216 squaredDistance = _cachedSeparatingAxis.LengthSquared();
217
218 if (previouseSquaredDistance - squaredDistance <= MathHelper.Epsilon * previouseSquaredDistance)
219 {
220 _simplexSolver.BackupClosest(out _cachedSeparatingAxis);
221 checkSimplex = true;
222 break;
223 }
224
225 if (_currentIteration++ > gjkMaxIter)
226 {
227#if DEBUG
228 Console.WriteLine("GjkPairDetector maxIter exceeded: {0}", _currentIteration);
229 Console.WriteLine("sepAxis=({0},{1},{2}), squaredDistance = {3}, shapeTypeA={4}, shapeTypeB={5}",
230 _cachedSeparatingAxis.X,
231 _cachedSeparatingAxis.Y,
232 _cachedSeparatingAxis.Z,
233 squaredDistance,
234 _minkowskiA.ShapeType,
235 _minkowskiB.ShapeType
236 );
237#endif
238 break;
239 }
240
241 bool check = (!_simplexSolver.FullSimplex);
242
243 if (!check)
244 {
245 _simplexSolver.BackupClosest(out _cachedSeparatingAxis);
246 break;
247 }
248 }
249
250 if (checkSimplex)
251 {
252 _simplexSolver.ComputePoints(out pointOnA, out pointOnB);
253 normalInB = pointOnA - pointOnB;
254 float lenSqr = _cachedSeparatingAxis.LengthSquared();
255
256 if (lenSqr < 0.0001f)
257 {
258 _degenerateSimplex = 5;
259 }
260
261 if (lenSqr > MathHelper.Epsilon * MathHelper.Epsilon)
262 {
263 float rlen = 1.0f / (float)Math.Sqrt((float)lenSqr);
264 normalInB *= rlen;
265 float s = (float)Math.Sqrt((float)squaredDistance);
266
267 BulletDebug.Assert(s > 0);
268 pointOnA -= _cachedSeparatingAxis * (marginA / s);
269 pointOnB += _cachedSeparatingAxis * (marginB / s);
270 distance = ((1 / rlen) - margin);
271
272 isValid = true;
273
274 _lastUsedMethod = 1;
275 }
276 else
277 {
278 _lastUsedMethod = 2;
279 }
280 }
281
282 bool catchDegeneratePenetrationCase =
283 (_catchDegeneracies != 0 && _penetrationDepthSolver != null && _degenerateSimplex != 0 && ((distance + margin) < 0.01f));
284
285 if (checkPenetration && (!isValid || catchDegeneratePenetrationCase))
286 {
287#warning Check this
288 if (_penetrationDepthSolver != null)
289 {
290 Vector3 tmpPointOnA, tmpPointOnB;
291
292 _numDeepPenetrationChecks++;
293
294 bool isValid2 = _penetrationDepthSolver.CalculatePenetrationDepth(
295 _simplexSolver, _minkowskiA, _minkowskiB, localTransA, localTransB,
296 _cachedSeparatingAxis, out tmpPointOnA, out tmpPointOnB,
297 debugDraw
298 );
299
300 if (isValid2)
301 {
302 Vector3 tmpNormalInB = tmpPointOnB - tmpPointOnA;
303 float lengSqr = tmpNormalInB.LengthSquared();
304
305 if (lengSqr > (MathHelper.Epsilon * MathHelper.Epsilon))
306 {
307 tmpNormalInB /= (float)Math.Sqrt((float)lengSqr);
308 float distance2 = -(tmpPointOnA - tmpPointOnB).Length();
309
310 if (!isValid || (distance2 < distance))
311 {
312 distance = distance2;
313 pointOnA = tmpPointOnA;
314 pointOnB = tmpPointOnB;
315 normalInB = tmpNormalInB;
316 isValid = true;
317 _lastUsedMethod = 3;
318 }
319 else
320 {
321
322 }
323 }
324 else
325 {
326 _lastUsedMethod = 4;
327 }
328 }
329 else
330 {
331 _lastUsedMethod = 5;
332 }
333 }
334 }
335
336 if (isValid)
337 {
338 output.AddContactPoint(normalInB, pointOnB + positionOffset, distance);
339 }
340 }
341 }
342 }
343} \ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs
new file mode 100644
index 0000000..59fd486
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/IConvexPenetrationDepthSolver.cs
@@ -0,0 +1,42 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// IConvexPenetrationDepthSolver provides an interface for penetration depth calculation.
31 /// </summary>
32 public interface IConvexPenetrationDepthSolver
33 {
34 bool CalculatePenetrationDepth(
35 ISimplexSolver simplexSolver,
36 ConvexShape convexA, ConvexShape convexB,
37 Matrix transformA, Matrix transformB,
38 Vector3 vector, out Vector3 ptrA, out Vector3 ptrB,
39 IDebugDraw debugDraw//, StackAlloc stackAlloc
40 );
41 }
42}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs
new file mode 100644
index 0000000..71b1eaa
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ISimplexSolver.cs
@@ -0,0 +1,48 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// ISimplexSolver can incrementally calculate distance between origin and up to 4 vertices
30 /// Used by GJK or Linear Casting. Can be implemented by the Johnson-algorithm or alternative approaches based on
31 /// voronoi regions or barycentric coordinates
32 public interface ISimplexSolver
33 {
34 void Reset();
35 void AddVertex(Vector3 w, Vector3 p, Vector3 q);
36 bool Closest(out Vector3 v);
37
38 int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf);
39 bool InSimplex(Vector3 w);
40 void BackupClosest(out Vector3 v);
41 void ComputePoints(out Vector3 pA, out Vector3 pB);
42
43 int NumVertices { get;}
44 bool EmptySimplex { get;}
45 float MaxVertex { get;}
46 bool FullSimplex { get;}
47 }
48} \ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs
new file mode 100644
index 0000000..751a9df
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/ManifoldPoint.cs
@@ -0,0 +1,78 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class ManifoldPoint
30 {
31 private Vector3 _localPointA;
32 private Vector3 _localPointB;
33 private Vector3 _positionWorldOnB;
34 private Vector3 _positionWorldOnA;
35 private Vector3 _normalWorldOnB;
36
37 private float _distance;
38 private float _combinedFriction;
39 private float _combinedRestitution;
40
41 private object _userPersistentData;
42
43 private int _lifeTime;//lifetime of the contactpoint in frames
44
45 public ManifoldPoint()
46 : this(new Vector3(), new Vector3(), new Vector3(), 0f)
47 {
48 }
49
50 public ManifoldPoint(Vector3 pointA, Vector3 pointB,
51 Vector3 normal,
52 float distance)
53 {
54 _localPointA = pointA;
55 _localPointB = pointB;
56 _normalWorldOnB = normal;
57 _distance = distance;
58 _positionWorldOnA = new Vector3();
59 _positionWorldOnB = new Vector3();
60 }
61
62 public float Distance { get { return _distance; } set { _distance = value; } }
63 public int LifeTime { get { return _lifeTime; } set { _lifeTime = value; } }
64
65 public Vector3 PositionWorldOnA { get { return _positionWorldOnA; } set { _positionWorldOnA = value; } }
66 public Vector3 PositionWorldOnB { get { return _positionWorldOnB; } set { _positionWorldOnB = value; } }
67
68 public Vector3 LocalPointA { get { return _localPointA; } set { _localPointA = value; } }
69 public Vector3 LocalPointB { get { return _localPointB; } set { _localPointB = value; } }
70
71 public Vector3 NormalWorldOnB { get { return _normalWorldOnB; } }
72
73 public float CombinedFriction { get { return _combinedFriction; } set { _combinedFriction = value; } }
74 public float CombinedRestitution { get { return _combinedRestitution; } set { _combinedRestitution = value; } }
75
76 public object UserPersistentData { get { return _userPersistentData; } set { _userPersistentData = value; } }
77 }
78}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs
new file mode 100644
index 0000000..81b05b8
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/MinkowskiPenetrationDepthSolver.cs
@@ -0,0 +1,246 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// MinkowskiPenetrationDepthSolver implements bruteforce penetration depth estimation.
31 /// Implementation is based on sampling the depth using support mapping, and using GJK step to get the witness points.
32 /// </summary>
33 public class MinkowskiPenetrationDepthSolver : IConvexPenetrationDepthSolver
34 {
35 private const int UnitSpherePointsCount = 42;
36
37 private static Vector3[] penetrationDirections =
38 {
39 new Vector3(0.000000f , -0.000000f,-1.000000f),
40 new Vector3(0.723608f , -0.525725f,-0.447219f),
41 new Vector3(-0.276388f , -0.850649f,-0.447219f),
42 new Vector3(-0.894426f , -0.000000f,-0.447216f),
43 new Vector3(-0.276388f , 0.850649f,-0.447220f),
44 new Vector3(0.723608f , 0.525725f,-0.447219f),
45 new Vector3(0.276388f , -0.850649f,0.447220f),
46 new Vector3(-0.723608f , -0.525725f,0.447219f),
47 new Vector3(-0.723608f , 0.525725f,0.447219f),
48 new Vector3(0.276388f , 0.850649f,0.447219f),
49 new Vector3(0.894426f , 0.000000f,0.447216f),
50 new Vector3(-0.000000f , 0.000000f,1.000000f),
51 new Vector3(0.425323f , -0.309011f,-0.850654f),
52 new Vector3(-0.162456f , -0.499995f,-0.850654f),
53 new Vector3(0.262869f , -0.809012f,-0.525738f),
54 new Vector3(0.425323f , 0.309011f,-0.850654f),
55 new Vector3(0.850648f , -0.000000f,-0.525736f),
56 new Vector3(-0.525730f , -0.000000f,-0.850652f),
57 new Vector3(-0.688190f , -0.499997f,-0.525736f),
58 new Vector3(-0.162456f , 0.499995f,-0.850654f),
59 new Vector3(-0.688190f , 0.499997f,-0.525736f),
60 new Vector3(0.262869f , 0.809012f,-0.525738f),
61 new Vector3(0.951058f , 0.309013f,0.000000f),
62 new Vector3(0.951058f , -0.309013f,0.000000f),
63 new Vector3(0.587786f , -0.809017f,0.000000f),
64 new Vector3(0.000000f , -1.000000f,0.000000f),
65 new Vector3(-0.587786f , -0.809017f,0.000000f),
66 new Vector3(-0.951058f , -0.309013f,-0.000000f),
67 new Vector3(-0.951058f , 0.309013f,-0.000000f),
68 new Vector3(-0.587786f , 0.809017f,-0.000000f),
69 new Vector3(-0.000000f , 1.000000f,-0.000000f),
70 new Vector3(0.587786f , 0.809017f,-0.000000f),
71 new Vector3(0.688190f , -0.499997f,0.525736f),
72 new Vector3(-0.262869f , -0.809012f,0.525738f),
73 new Vector3(-0.850648f , 0.000000f,0.525736f),
74 new Vector3(-0.262869f , 0.809012f,0.525738f),
75 new Vector3(0.688190f , 0.499997f,0.525736f),
76 new Vector3(0.525730f , 0.000000f,0.850652f),
77 new Vector3(0.162456f , -0.499995f,0.850654f),
78 new Vector3(-0.425323f , -0.309011f,0.850654f),
79 new Vector3(-0.425323f , 0.309011f,0.850654f),
80 new Vector3(0.162456f , 0.499995f,0.850654f)
81 };
82
83 private class IntermediateResult : DiscreteCollisionDetectorInterface.Result
84 {
85 private Vector3 _normalOnBInWorld;
86 private Vector3 _pointInWorld;
87 private float _depth;
88 private bool _hasResult;
89
90 public IntermediateResult()
91 {
92 _hasResult = false;
93 }
94
95 public bool HasResult { get { return _hasResult; } }
96 public float Depth { get { return _depth; } }
97 public Vector3 PointInWorld { get { return _pointInWorld; } }
98
99 public override void SetShapeIdentifiers(int partId0, int index0, int partId1, int index1)
100 {
101 }
102
103 public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth)
104 {
105 _normalOnBInWorld = normalOnBInWorld;
106 _pointInWorld = pointInWorld;
107 _depth = depth;
108 _hasResult = true;
109 }
110 }
111
112 #region IConvexPenetrationDepthSolver Members
113 public bool CalculatePenetrationDepth(ISimplexSolver simplexSolver,
114 ConvexShape convexA, ConvexShape convexB,
115 Matrix transformA, Matrix transformB,
116 Vector3 v, out Vector3 pa, out Vector3 pb, IDebugDraw debugDraw)
117 {
118 pa = new Vector3();
119 pb = new Vector3();
120 //just take fixed number of orientation, and sample the penetration depth in that direction
121 float minProj = 1e30f;
122 Vector3 minNorm = new Vector3();
123 Vector3 minA = new Vector3(), minB = new Vector3();
124 Vector3 seperatingAxisInA, seperatingAxisInB;
125 Vector3 pInA, qInB, pWorld, qWorld, w;
126
127 Vector3[] supportVerticesABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
128 Vector3[] supportVerticesBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
129 Vector3[] seperatingAxisInABatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
130 Vector3[] seperatingAxisInBBatch = new Vector3[UnitSpherePointsCount + ConvexShape.MaxPreferredPenetrationDirections * 2];
131
132 int numSampleDirections = UnitSpherePointsCount;
133
134 for (int i = 0; i < numSampleDirections; i++)
135 {
136 Vector3 norm = penetrationDirections[i];
137 seperatingAxisInABatch[i] = Vector3.TransformNormal((-norm), transformA);
138 seperatingAxisInBBatch[i] = Vector3.TransformNormal(norm, transformB);
139 }
140
141 {
142 int numPDA = convexA.PreferredPenetrationDirectionsCount;
143 if (numPDA != 0)
144 {
145 for (int i = 0; i < numPDA; i++)
146 {
147 Vector3 norm;
148 convexA.GetPreferredPenetrationDirection(i, out norm);
149 norm = Vector3.TransformNormal(norm, transformA);
150 penetrationDirections[numSampleDirections] = norm;
151 seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA);
152 seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB);
153 numSampleDirections++;
154 }
155 }
156 }
157
158 {
159 int numPDB = convexB.PreferredPenetrationDirectionsCount;
160 if (numPDB != 0)
161 {
162 for (int i = 0; i < numPDB; i++)
163 {
164 Vector3 norm;
165 convexB.GetPreferredPenetrationDirection(i, out norm);
166 norm = Vector3.TransformNormal(norm, transformB);
167 penetrationDirections[numSampleDirections] = norm;
168 seperatingAxisInABatch[numSampleDirections] = Vector3.TransformNormal((-norm), transformA);
169 seperatingAxisInBBatch[numSampleDirections] = Vector3.TransformNormal(norm, transformB);
170 numSampleDirections++;
171 }
172 }
173 }
174
175 convexA.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInABatch, supportVerticesABatch); //, numSampleDirections);
176 convexB.BatchedUnitVectorGetSupportingVertexWithoutMargin(seperatingAxisInBBatch, supportVerticesBBatch); //, numSampleDirections);
177
178 for (int i = 0; i < numSampleDirections; i++)
179 {
180 Vector3 norm = penetrationDirections[i];
181 seperatingAxisInA = seperatingAxisInABatch[i];
182 seperatingAxisInB = seperatingAxisInBBatch[i];
183
184 pInA = supportVerticesABatch[i];
185 qInB = supportVerticesBBatch[i];
186
187 pWorld = MathHelper.MatrixToVector(transformA, pInA);
188 qWorld = MathHelper.MatrixToVector(transformB, qInB);
189 w = qWorld - pWorld;
190 float delta = Vector3.Dot(norm, w);
191 //find smallest delta
192 if (delta < minProj)
193 {
194 minProj = delta;
195 minNorm = norm;
196 minA = pWorld;
197 minB = qWorld;
198 }
199 }
200
201 //add the margins
202 minA += minNorm * convexA.Margin;
203 minB -= minNorm * convexB.Margin;
204 //no penetration
205 if (minProj < 0)
206 return false;
207
208 minProj += (convexA.Margin + convexB.Margin);
209
210 GjkPairDetector gjkdet = new GjkPairDetector(convexA, convexB, simplexSolver, null);
211
212 float offsetDist = minProj;
213 Vector3 offset = minNorm * offsetDist;
214
215 GjkPairDetector.ClosestPointInput input = new DiscreteCollisionDetectorInterface.ClosestPointInput();
216
217 Vector3 newOrg = transformA.Translation + offset;
218
219 Matrix displacedTrans = transformA;
220 displacedTrans.Translation = newOrg;
221
222 input.TransformA = displacedTrans;
223 input.TransformB = transformB;
224 input.MaximumDistanceSquared = 1e30f;//minProj;
225
226 IntermediateResult res = new IntermediateResult();
227 gjkdet.GetClosestPoints(input, res, debugDraw);
228
229 float correctedMinNorm = minProj - res.Depth;
230
231 //the penetration depth is over-estimated, relax it
232 float penetration_relaxation = 1;
233 minNorm *= penetration_relaxation;
234
235 if (res.HasResult)
236 {
237
238 pa = res.PointInWorld - minNorm * correctedMinNorm;
239 pb = res.PointInWorld;
240 }
241
242 return res.HasResult;
243 }
244 #endregion
245 }
246}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs
new file mode 100644
index 0000000..47cfe9a
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PersistentManifold.cs
@@ -0,0 +1,272 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public delegate bool ContactDestroyedCallback(object userPersistentData);
30
31 public class PersistentManifold
32 {
33 private static ContactDestroyedCallback _contactDestroyedCallback = null;
34 private static float _contactBreakingThreshold = 0.02f;
35
36 private ManifoldPoint[] _pointCache = new ManifoldPoint[4];
37
38 // this two body pointers can point to the physics rigidbody class.
39 // object will allow any rigidbody class
40 private object _bodyA;
41 private object _bodyB;
42 private int _cachedPoints;
43
44 public PersistentManifold(object bodyA, object bodyB)
45 {
46 _bodyA = bodyA;
47 _bodyB = bodyB;
48 _cachedPoints = 0;
49 }
50
51 public object BodyA { get { return _bodyA; } }
52 public object BodyB { get { return _bodyB; } }
53
54 public int ContactsCount { get { return _cachedPoints; } }
55
56 public static ContactDestroyedCallback ContactDestroyedCallback { get { return _contactDestroyedCallback; } set { _contactDestroyedCallback = value; } }
57 public static float ContactBreakingThreshold { get { return _contactBreakingThreshold; } }
58
59 public void SetBodies(object bodyA, object bodyB)
60 {
61 _bodyA = bodyA;
62 _bodyB = bodyB;
63 }
64
65 public ManifoldPoint GetContactPoint(int index)
66 {
67 if (index >= _cachedPoints)
68 throw new ArgumentOutOfRangeException("index", "index must be smaller than cachedPoints");
69
70 return _pointCache[index];
71 }
72
73 public int GetCacheEntry(ManifoldPoint newPoint)
74 {
75 float shortestDist = ContactBreakingThreshold * ContactBreakingThreshold;
76 int size = ContactsCount;
77 int nearestPoint = -1;
78 for (int i = 0; i < size; i++)
79 {
80 ManifoldPoint mp = _pointCache[i];
81
82 Vector3 diffA = mp.LocalPointA - newPoint.LocalPointA;
83 float distToManiPoint = Vector3.Dot(diffA, diffA);
84 if (distToManiPoint < shortestDist)
85 {
86 shortestDist = distToManiPoint;
87 nearestPoint = i;
88 }
89 }
90 return nearestPoint;
91 }
92
93 public void AddManifoldPoint(ManifoldPoint newPoint)
94 {
95 if (!ValidContactDistance(newPoint))
96 throw new BulletException();
97
98 int insertIndex = ContactsCount;
99 if (insertIndex == 4)
100 {
101 //sort cache so best points come first, based on area
102 insertIndex = SortCachedPoints(newPoint);
103 }
104 else
105 {
106 _cachedPoints++;
107 }
108 ReplaceContactPoint(newPoint, insertIndex);
109 }
110
111 public void RemoveContactPoint(int index)
112 {
113 ClearUserCache(_pointCache[index]);
114
115 int lastUsedIndex = ContactsCount - 1;
116 _pointCache[index] = _pointCache[lastUsedIndex];
117 //get rid of duplicated userPersistentData pointer
118 _pointCache[lastUsedIndex].UserPersistentData = null;
119 _cachedPoints--;
120 }
121
122 public void ReplaceContactPoint(ManifoldPoint newPoint, int insertIndex)
123 {
124 BulletDebug.Assert(ValidContactDistance(newPoint));
125
126 if (_pointCache[insertIndex] != null)
127 {
128 int lifeTime = _pointCache[insertIndex].LifeTime;
129 BulletDebug.Assert(lifeTime >= 0);
130 object cache = _pointCache[insertIndex].UserPersistentData;
131
132 _pointCache[insertIndex] = newPoint;
133
134 _pointCache[insertIndex].UserPersistentData = cache;
135 _pointCache[insertIndex].LifeTime = lifeTime;
136 }
137 else
138 {
139 _pointCache[insertIndex] = newPoint;
140 }
141
142 //ClearUserCache(_pointCache[insertIndex]);
143 //_pointCache[insertIndex] = newPoint;
144 }
145
146 public bool ValidContactDistance(ManifoldPoint pt)
147 {
148 return pt.Distance <= ContactBreakingThreshold;
149 }
150
151 // calculated new worldspace coordinates and depth, and reject points that exceed the collision margin
152 public void RefreshContactPoints(Matrix trA, Matrix trB)
153 {
154 // first refresh worldspace positions and distance
155 for (int i = ContactsCount - 1; i >= 0; i--)
156 {
157 ManifoldPoint manifoldPoint = _pointCache[i];
158 manifoldPoint.PositionWorldOnA = MathHelper.MatrixToVector(trA,manifoldPoint.LocalPointA);
159 manifoldPoint.PositionWorldOnB = MathHelper.MatrixToVector(trB, manifoldPoint.LocalPointB);
160 manifoldPoint.Distance = Vector3.Dot(manifoldPoint.PositionWorldOnA - manifoldPoint.PositionWorldOnB, manifoldPoint.NormalWorldOnB);
161 manifoldPoint.LifeTime++;
162 }
163
164 // then
165 float distance2d;
166 Vector3 projectedDifference, projectedPoint;
167 for (int i = ContactsCount - 1; i >= 0; i--)
168 {
169
170 ManifoldPoint manifoldPoint = _pointCache[i];
171 //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
172 if (!ValidContactDistance(manifoldPoint))
173 {
174 RemoveContactPoint(i);
175 }
176 else
177 {
178 //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
179 projectedPoint = manifoldPoint.PositionWorldOnA - manifoldPoint.NormalWorldOnB * manifoldPoint.Distance;
180 projectedDifference = manifoldPoint.PositionWorldOnB - projectedPoint;
181 distance2d = Vector3.Dot(projectedDifference, projectedDifference);
182 if (distance2d > ContactBreakingThreshold * ContactBreakingThreshold)
183 {
184 RemoveContactPoint(i);
185 }
186 }
187 }
188 }
189
190 public void ClearManifold()
191 {
192 for (int i = 0; i < _cachedPoints; i++)
193 {
194 ClearUserCache(_pointCache[i]);
195 }
196 _cachedPoints = 0;
197 }
198
199 private void ClearUserCache(ManifoldPoint pt)
200 {
201 if (pt != null)
202 {
203 object oldPtr = pt.UserPersistentData;
204
205 if (oldPtr != null)
206 {
207 if (pt.UserPersistentData != null && _contactDestroyedCallback != null)
208 {
209 _contactDestroyedCallback(pt.UserPersistentData);
210 pt.UserPersistentData = null;
211 }
212 }
213 }
214 }
215
216 // sort cached points so most isolated points come first
217 private int SortCachedPoints(ManifoldPoint pt)
218 {
219 //calculate 4 possible cases areas, and take biggest area
220 //also need to keep 'deepest'
221
222 int maxPenetrationIndex = -1;
223 float maxPenetration = pt.Distance;
224 for (int i = 0; i < 4; i++)
225 {
226 if (_pointCache[i].Distance < maxPenetration)
227 {
228 maxPenetrationIndex = i;
229 maxPenetration = _pointCache[i].Distance;
230 }
231 }
232
233 float res0 = 0, res1 = 0, res2 = 0, res3 = 0;
234 if (maxPenetrationIndex != 0)
235 {
236 Vector3 a0 = pt.LocalPointA - _pointCache[1].LocalPointA;
237 Vector3 b0 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA;
238 Vector3 cross = Vector3.Cross(a0, b0);
239 res0 = cross.LengthSquared();
240 }
241 if (maxPenetrationIndex != 1)
242 {
243 Vector3 a1 = pt.LocalPointA - _pointCache[0].LocalPointA;
244 Vector3 b1 = _pointCache[3].LocalPointA - _pointCache[2].LocalPointA;
245 Vector3 cross = Vector3.Cross(a1, b1);
246 res1 = cross.LengthSquared();
247 }
248
249 if (maxPenetrationIndex != 2)
250 {
251 Vector3 a2 = pt.LocalPointA - _pointCache[0].LocalPointA;
252 Vector3 b2 = _pointCache[3].LocalPointA - _pointCache[1].LocalPointA;
253 Vector3 cross = Vector3.Cross(a2, b2);
254 res2 = cross.LengthSquared();
255 }
256
257 if (maxPenetrationIndex != 3)
258 {
259 Vector3 a3 = pt.LocalPointA - _pointCache[0].LocalPointA;
260 Vector3 b3 = _pointCache[2].LocalPointA - _pointCache[1].LocalPointA;
261 Vector3 cross = Vector3.Cross(a3, b3);
262 res3 = cross.LengthSquared();
263 }
264
265 Vector4 maxvec = new Vector4(res0, res1, res2, res3);
266 int biggestarea = MathHelper.ClosestAxis(maxvec);
267 return biggestarea;
268 }
269
270 private int FindContactPoint(ManifoldPoint unUsed, int numUnused, ManifoldPoint pt) { return 0; }
271 }
272}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs
new file mode 100644
index 0000000..4e0b0a0
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/PointCollector.cs
@@ -0,0 +1,64 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class PointCollector : DiscreteCollisionDetectorInterface.Result
30 {
31 private Vector3 _normalOnBInWorld;
32 private Vector3 _pointInWorld;
33 private float _distance; //negative means penetration
34 private bool _hasResult;
35
36 public PointCollector()
37 {
38 _distance = 1e30f;
39 _hasResult = false;
40 }
41
42 public Vector3 NormalOnBInWorld { get { return _normalOnBInWorld; } }
43 public Vector3 PointInWorld { get { return _pointInWorld; } }
44 public float Distance { get { return _distance; } }
45 public bool HasResult { get { return _hasResult; } }
46
47 public override void SetShapeIdentifiers(int partIdA, int indexA, int partIdB, int indexB)
48 {
49 //??
50 }
51
52 public override void AddContactPoint(Vector3 normalOnBInWorld, Vector3 pointInWorld, float depth)
53 {
54 if (depth < _distance)
55 {
56 _hasResult = true;
57 _normalOnBInWorld = normalOnBInWorld;
58 _pointInWorld = pointInWorld;
59 //negative means penetration
60 _distance = depth;
61 }
62 }
63 }
64}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs
new file mode 100644
index 0000000..6d320c4
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/SubsimplexConvexCast.cs
@@ -0,0 +1,142 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 /// <summary>
30 /// SubsimplexConvexCast implements Gino van den Bergens' paper
31 /// "Ray Casting against bteral Convex Objects with Application to Continuous Collision Detection"
32 /// GJK based Ray Cast, optimized version
33 /// Objects should not start in overlap, otherwise results are not defined.
34 /// </summary>
35 public class SubsimplexConvexCast : IConvexCast
36 {
37 private ISimplexSolver _simplexSolver;
38 private ConvexShape _convexA;
39 private ConvexShape _convexB;
40
41 /// <summary>
42 /// Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.
43 /// See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
44 /// </summary>
45 private const int MaxIterations = 32;
46
47 public SubsimplexConvexCast(ConvexShape shapeA, ConvexShape shapeB, ISimplexSolver simplexSolver)
48 {
49 _simplexSolver = simplexSolver;
50 _convexA = shapeA;
51 _convexB = shapeB;
52 }
53
54 #region IConvexCast Members
55 /// <summary>
56 /// SimsimplexConvexCast calculateTimeOfImpact calculates the time of impact+normal for the linear cast (sweep) between two moving objects.
57 /// Precondition is that objects should not penetration/overlap at the start from the interval. Overlap can be tested using GjkPairDetector.
58 /// </summary>
59 /// <param name="fromA"></param>
60 /// <param name="toA"></param>
61 /// <param name="fromB"></param>
62 /// <param name="toB"></param>
63 /// <param name="result"></param>
64 /// <returns></returns>
65 public bool CalcTimeOfImpact(Matrix fromA, Matrix toA, Matrix fromB, Matrix toB, CastResult result)
66 {
67 MinkowskiSumShape convex = new MinkowskiSumShape(_convexA, _convexB);
68
69 Matrix rayFromLocalA;
70 Matrix rayToLocalA;
71
72 rayFromLocalA = MathHelper.InvertMatrix(fromA) * fromB;
73 rayToLocalA = MathHelper.InvertMatrix(toA) * toB;
74
75 _simplexSolver.Reset();
76
77 convex.TransformB = rayFromLocalA;
78
79 float lambda = 0;
80 //todo: need to verify this:
81 //because of minkowski difference, we need the inverse direction
82
83 Vector3 s = -rayFromLocalA.Translation;
84 Vector3 r = -(rayToLocalA.Translation - rayFromLocalA.Translation);
85 Vector3 x = s;
86 Vector3 v;
87 Vector3 arbitraryPoint = convex.LocalGetSupportingVertex(r);
88
89 v = x - arbitraryPoint;
90
91 int maxIter = MaxIterations;
92
93 Vector3 n = new Vector3();
94 float lastLambda = lambda;
95
96 float dist2 = v.LengthSquared();
97 float epsilon = 0.0001f;
98
99 Vector3 w, p;
100 float VdotR;
101
102 while ((dist2 > epsilon) && (maxIter-- != 0))
103 {
104 p = convex.LocalGetSupportingVertex(v);
105 w = x - p;
106
107 float VdotW = Vector3.Dot(v, w);
108
109 if (VdotW > 0)
110 {
111 VdotR = Vector3.Dot(v, r);
112
113 if (VdotR >= -(MathHelper.Epsilon * MathHelper.Epsilon))
114 return false;
115 else
116 {
117 lambda = lambda - VdotW / VdotR;
118 x = s + lambda * r;
119 _simplexSolver.Reset();
120 //check next line
121 w = x - p;
122 lastLambda = lambda;
123 n = v;
124 }
125 }
126 _simplexSolver.AddVertex(w, x, p);
127 if (_simplexSolver.Closest(out v))
128 {
129 dist2 = v.LengthSquared();
130 }
131 else
132 {
133 dist2 = 0f;
134 }
135 }
136 result.Fraction = lambda;
137 result.Normal = n;
138 return true;
139 }
140 #endregion
141 }
142}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs
new file mode 100644
index 0000000..c127460
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/TriangleRaycastCallback.cs
@@ -0,0 +1,115 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public abstract class TriangleRaycastCallback : ITriangleCallback
30 {
31 private Vector3 _from;
32 private Vector3 _to;
33 private float _hitFraction;
34
35 public TriangleRaycastCallback(Vector3 from, Vector3 to)
36 {
37 _from = from;
38 _to = to;
39 _hitFraction = 1;
40 }
41
42 public Vector3 From { get { return _from; } set { _from = value; } }
43 public Vector3 To { get { return _to; } set { _to = value; } }
44 public float HitFraction { get { return _hitFraction; } set { _hitFraction = value; } }
45
46 public abstract float ReportHit(Vector3 hitNormalLocal, float hitFraction, int partId, int triangleIndex);
47
48 #region ITriangleCallback Members
49
50 public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex)
51 {
52 Vector3 vertA = triangle[0];
53 Vector3 vertB = triangle[1];
54 Vector3 vertC = triangle[2];
55
56 Vector3 vBA = vertB - vertA;
57 Vector3 vCA = vertC - vertA;
58
59 Vector3 triangleNormal = Vector3.Cross(vBA, vCA);
60
61 float dist = Vector3.Dot(vertA, triangleNormal);
62 float distA = Vector3.Dot(triangleNormal, _from);
63 distA -= dist;
64 float distB = Vector3.Dot(triangleNormal, _to);
65 distB -= dist;
66
67 if (distA * distB >= 0.0f)
68 {
69 return; // same sign
70 }
71
72 float projLength = distA - distB;
73 float distance = (distA) / (projLength);
74 // Now we have the intersection point on the plane, we'll see if it's inside the triangle
75 // Add an epsilon as a tolerance for the raycast,
76 // in case the ray hits exacly on the edge of the triangle.
77 // It must be scaled for the triangle size.
78
79 if (distance < _hitFraction)
80 {
81 float edgeTolerance = triangleNormal.LengthSquared();
82 edgeTolerance *= -0.0001f;
83 Vector3 point = new Vector3();
84 MathHelper.SetInterpolate3(_from, _to, distance, ref point);
85
86 Vector3 vertexAPoint = vertA - point;
87 Vector3 vertexBPoint = vertB - point;
88 Vector3 contactPointA = Vector3.Cross(vertexAPoint, vertexBPoint);
89
90 if (Vector3.Dot(contactPointA, triangleNormal) >= edgeTolerance)
91 {
92 Vector3 vertexCPoint = vertC - point;
93 Vector3 contactPointB = Vector3.Cross(vertexBPoint, vertexCPoint);
94 if (Vector3.Dot(contactPointB, triangleNormal) >= edgeTolerance)
95 {
96 Vector3 contactPointC = Vector3.Cross(vertexCPoint, vertexAPoint);
97
98 if (Vector3.Dot(contactPointC, triangleNormal) >= edgeTolerance)
99 {
100 if (distA > 0)
101 {
102 _hitFraction = ReportHit(triangleNormal, distance, partID, triangleIndex);
103 }
104 else
105 {
106 _hitFraction = ReportHit(-triangleNormal, distance, partID, triangleIndex);
107 }
108 }
109 }
110 }
111 }
112 }
113 #endregion
114 }
115}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs
new file mode 100644
index 0000000..16f3dab
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Collision/NarrowPhaseCollision/VoronoiSimplexSolver.cs
@@ -0,0 +1,643 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public class UsageBitfield
30 {
31 private bool _usedVertexA, _usedVertexB, _usedVertexC, _usedVertexD;
32
33 public bool UsedVertexA { get { return _usedVertexA; } set { _usedVertexA = value; } }
34 public bool UsedVertexB { get { return _usedVertexB; } set { _usedVertexB = value; } }
35 public bool UsedVertexC { get { return _usedVertexC; } set { _usedVertexC = value; } }
36 public bool UsedVertexD { get { return _usedVertexD; } set { _usedVertexD = value; } }
37
38 public void Reset()
39 {
40 _usedVertexA = _usedVertexB = _usedVertexC = _usedVertexD = false;
41 }
42 }
43
44 public class SubSimplexClosestResult
45 {
46 private Vector3 _closestPointOnSimplex;
47
48 //MASK for m_usedVertices
49 //stores the simplex vertex-usage, using the MASK,
50 // if m_usedVertices & MASK then the related vertex is used
51 private UsageBitfield _usedVertices = new UsageBitfield();
52 private float[] _barycentricCoords = new float[4];
53 private bool _degenerate;
54
55 public Vector3 ClosestPointOnSimplex { get { return _closestPointOnSimplex; } set { _closestPointOnSimplex = value; } }
56 public UsageBitfield UsedVertices { get { return _usedVertices; } set { _usedVertices = value; } }
57 public float[] BarycentricCoords { get { return _barycentricCoords; } set { _barycentricCoords = value; } }
58 public bool Degenerate { get { return _degenerate; } set { _degenerate = value; } }
59
60 public void Reset()
61 {
62 _degenerate = false;
63 SetBarycentricCoordinates();
64 _usedVertices.Reset();
65 }
66
67 public bool IsValid
68 {
69 get
70 {
71 return (_barycentricCoords[0] >= 0f) &&
72 (_barycentricCoords[1] >= 0f) &&
73 (_barycentricCoords[2] >= 0f) &&
74 (_barycentricCoords[3] >= 0f);
75 }
76 }
77
78 public void SetBarycentricCoordinates()
79 {
80 SetBarycentricCoordinates(0f, 0f, 0f, 0f);
81 }
82
83 public void SetBarycentricCoordinates(float a, float b, float c, float d)
84 {
85 _barycentricCoords[0] = a;
86 _barycentricCoords[1] = b;
87 _barycentricCoords[2] = c;
88 _barycentricCoords[3] = d;
89 }
90 }
91
92 /// VoronoiSimplexSolver is an implementation of the closest point distance
93 /// algorithm from a 1-4 points simplex to the origin.
94 /// Can be used with GJK, as an alternative to Johnson distance algorithm.
95 public class VoronoiSimplexSolver : ISimplexSolver
96 {
97 private const int VertexA = 0, VertexB = 1, VertexC = 2, VertexD = 3;
98
99 private const int VoronoiSimplexMaxVerts = 5;
100 private const bool CatchDegenerateTetrahedron = true;
101
102 private int _numVertices;
103
104 private Vector3[] _simplexVectorW = new Vector3[VoronoiSimplexMaxVerts];
105 private Vector3[] _simplexPointsP = new Vector3[VoronoiSimplexMaxVerts];
106 private Vector3[] _simplexPointsQ = new Vector3[VoronoiSimplexMaxVerts];
107
108 private Vector3 _cachedPA;
109 private Vector3 _cachedPB;
110 private Vector3 _cachedV;
111 private Vector3 _lastW;
112 private bool _cachedValidClosest;
113
114 private SubSimplexClosestResult _cachedBC = new SubSimplexClosestResult();
115
116 private bool _needsUpdate;
117
118 #region ISimplexSolver Members
119
120 public bool FullSimplex
121 {
122 get
123 {
124 return _numVertices == 4;
125 }
126 }
127
128 public int NumVertices
129 {
130 get
131 {
132 return _numVertices;
133 }
134 }
135
136 public void Reset()
137 {
138 _cachedValidClosest = false;
139 _numVertices = 0;
140 _needsUpdate = true;
141 _lastW = new Vector3(1e30f, 1e30f, 1e30f);
142 _cachedBC.Reset();
143 }
144
145 public void AddVertex(Vector3 w, Vector3 p, Vector3 q)
146 {
147 _lastW = w;
148 _needsUpdate = true;
149
150 _simplexVectorW[_numVertices] = w;
151 _simplexPointsP[_numVertices] = p;
152 _simplexPointsQ[_numVertices] = q;
153
154 _numVertices++;
155 }
156
157 //return/calculate the closest vertex
158 public bool Closest(out Vector3 v)
159 {
160 bool succes = UpdateClosestVectorAndPoints();
161 v = _cachedV;
162 return succes;
163 }
164
165 public float MaxVertex
166 {
167 get
168 {
169 int numverts = NumVertices;
170 float maxV = 0f, curLen2;
171 for (int i = 0; i < numverts; i++)
172 {
173 curLen2 = _simplexVectorW[i].LengthSquared();
174 if (maxV < curLen2) maxV = curLen2;
175 }
176 return maxV;
177 }
178 }
179
180 //return the current simplex
181 public int GetSimplex(out Vector3[] pBuf, out Vector3[] qBuf, out Vector3[] yBuf)
182 {
183 int numverts = NumVertices;
184 pBuf = new Vector3[numverts];
185 qBuf = new Vector3[numverts];
186 yBuf = new Vector3[numverts];
187 for (int i = 0; i < numverts; i++)
188 {
189 yBuf[i] = _simplexVectorW[i];
190 pBuf[i] = _simplexPointsP[i];
191 qBuf[i] = _simplexPointsQ[i];
192 }
193 return numverts;
194 }
195
196 public bool InSimplex(Vector3 w)
197 {
198 //check in case lastW is already removed
199 if (w == _lastW) return true;
200
201 //w is in the current (reduced) simplex
202 int numverts = NumVertices;
203 for (int i = 0; i < numverts; i++)
204 if (_simplexVectorW[i] == w) return true;
205
206 return false;
207 }
208
209 public void BackupClosest(out Vector3 v)
210 {
211 v = _cachedV;
212 }
213
214 public bool EmptySimplex
215 {
216 get
217 {
218 return NumVertices == 0;
219 }
220 }
221
222 public void ComputePoints(out Vector3 p1, out Vector3 p2)
223 {
224 UpdateClosestVectorAndPoints();
225 p1 = _cachedPA;
226 p2 = _cachedPB;
227 }
228
229 #endregion
230
231 public void RemoveVertex(int index)
232 {
233 BulletDebug.Assert(_numVertices > 0);
234 _numVertices--;
235 _simplexVectorW[index] = _simplexVectorW[_numVertices];
236 _simplexPointsP[index] = _simplexPointsP[_numVertices];
237 _simplexPointsQ[index] = _simplexPointsQ[_numVertices];
238 }
239
240 public void ReduceVertices(UsageBitfield usedVerts)
241 {
242 if ((NumVertices >= 4) && (!usedVerts.UsedVertexD)) RemoveVertex(3);
243 if ((NumVertices >= 3) && (!usedVerts.UsedVertexC)) RemoveVertex(2);
244 if ((NumVertices >= 2) && (!usedVerts.UsedVertexB)) RemoveVertex(1);
245 if ((NumVertices >= 1) && (!usedVerts.UsedVertexA)) RemoveVertex(0);
246 }
247
248 public bool UpdateClosestVectorAndPoints()
249 {
250 if (_needsUpdate)
251 {
252 _cachedBC.Reset();
253 _needsUpdate = false;
254
255 Vector3 p, a, b, c, d;
256 switch (NumVertices)
257 {
258 case 0:
259 _cachedValidClosest = false;
260 break;
261 case 1:
262 _cachedPA = _simplexPointsP[0];
263 _cachedPB = _simplexPointsQ[0];
264 _cachedV = _cachedPA - _cachedPB;
265 _cachedBC.Reset();
266 _cachedBC.SetBarycentricCoordinates(1f, 0f, 0f, 0f);
267 _cachedValidClosest = _cachedBC.IsValid;
268 break;
269 case 2:
270 //closest point origin from line segment
271 Vector3 from = _simplexVectorW[0];
272 Vector3 to = _simplexVectorW[1];
273 Vector3 nearest;
274
275 Vector3 diff = -from;
276 Vector3 v = to - from;
277 float t = Vector3.Dot(v, diff);
278
279 if (t > 0)
280 {
281 float dotVV = v.LengthSquared();
282 if (t < dotVV)
283 {
284 t /= dotVV;
285 diff -= t * v;
286 _cachedBC.UsedVertices.UsedVertexA = true;
287 _cachedBC.UsedVertices.UsedVertexB = true;
288 }
289 else
290 {
291 t = 1;
292 diff -= v;
293 //reduce to 1 point
294 _cachedBC.UsedVertices.UsedVertexB = true;
295 }
296 }
297 else
298 {
299 t = 0;
300 //reduce to 1 point
301 _cachedBC.UsedVertices.UsedVertexA = true;
302 }
303
304 _cachedBC.SetBarycentricCoordinates(1 - t, t, 0, 0);
305 nearest = from + t * v;
306
307 _cachedPA = _simplexPointsP[0] + t * (_simplexPointsP[1] - _simplexPointsP[0]);
308 _cachedPB = _simplexPointsQ[0] + t * (_simplexPointsQ[1] - _simplexPointsQ[0]);
309 _cachedV = _cachedPA - _cachedPB;
310
311 ReduceVertices(_cachedBC.UsedVertices);
312
313 _cachedValidClosest = _cachedBC.IsValid;
314 break;
315 case 3:
316 //closest point origin from triangle
317 p = new Vector3();
318 a = _simplexVectorW[0];
319 b = _simplexVectorW[1];
320 c = _simplexVectorW[2];
321
322 ClosestPtPointTriangle(p, a, b, c, ref _cachedBC);
323 _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] +
324 _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] +
325 _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] +
326 _simplexPointsP[3] * _cachedBC.BarycentricCoords[3];
327
328 _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] +
329 _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] +
330 _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] +
331 _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3];
332
333 _cachedV = _cachedPA - _cachedPB;
334
335 ReduceVertices(_cachedBC.UsedVertices);
336 _cachedValidClosest = _cachedBC.IsValid;
337 break;
338 case 4:
339 p = new Vector3();
340 a = _simplexVectorW[0];
341 b = _simplexVectorW[1];
342 c = _simplexVectorW[2];
343 d = _simplexVectorW[3];
344
345 bool hasSeperation = ClosestPtPointTetrahedron(p, a, b, c, d, ref _cachedBC);
346
347 if (hasSeperation)
348 {
349 _cachedPA = _simplexPointsP[0] * _cachedBC.BarycentricCoords[0] +
350 _simplexPointsP[1] * _cachedBC.BarycentricCoords[1] +
351 _simplexPointsP[2] * _cachedBC.BarycentricCoords[2] +
352 _simplexPointsP[3] * _cachedBC.BarycentricCoords[3];
353
354 _cachedPB = _simplexPointsQ[0] * _cachedBC.BarycentricCoords[0] +
355 _simplexPointsQ[1] * _cachedBC.BarycentricCoords[1] +
356 _simplexPointsQ[2] * _cachedBC.BarycentricCoords[2] +
357 _simplexPointsQ[3] * _cachedBC.BarycentricCoords[3];
358
359 _cachedV = _cachedPA - _cachedPB;
360 ReduceVertices(_cachedBC.UsedVertices);
361 }
362 else
363 {
364 if (_cachedBC.Degenerate)
365 {
366 _cachedValidClosest = false;
367 }
368 else
369 {
370 _cachedValidClosest = true;
371 //degenerate case == false, penetration = true + zero
372 _cachedV.X = _cachedV.Y = _cachedV.Z = 0f;
373 }
374 break; // !!!!!!!!!!!! proverit na vsakiy sluchai
375 }
376
377 _cachedValidClosest = _cachedBC.IsValid;
378
379 //closest point origin from tetrahedron
380 break;
381 default:
382 _cachedValidClosest = false;
383 break;
384 }
385 }
386
387 return _cachedValidClosest;
388 }
389
390 public bool ClosestPtPointTriangle(Vector3 p, Vector3 a, Vector3 b, Vector3 c,
391 ref SubSimplexClosestResult result)
392 {
393 result.UsedVertices.Reset();
394
395 float v, w;
396
397 // Check if P in vertex region outside A
398 Vector3 ab = b - a;
399 Vector3 ac = c - a;
400 Vector3 ap = p - a;
401 float d1 = Vector3.Dot(ab, ap);
402 float d2 = Vector3.Dot(ac, ap);
403 if (d1 <= 0f && d2 <= 0f)
404 {
405 result.ClosestPointOnSimplex = a;
406 result.UsedVertices.UsedVertexA = true;
407 result.SetBarycentricCoordinates(1, 0, 0, 0);
408 return true; // a; // barycentric coordinates (1,0,0)
409 }
410
411 // Check if P in vertex region outside B
412 Vector3 bp = p - b;
413 float d3 = Vector3.Dot(ab, bp);
414 float d4 = Vector3.Dot(ac, bp);
415 if (d3 >= 0f && d4 <= d3)
416 {
417 result.ClosestPointOnSimplex = b;
418 result.UsedVertices.UsedVertexB = true;
419 result.SetBarycentricCoordinates(0, 1, 0, 0);
420
421 return true; // b; // barycentric coordinates (0,1,0)
422 }
423 // Check if P in edge region of AB, if so return projection of P onto AB
424 float vc = d1 * d4 - d3 * d2;
425 if (vc <= 0f && d1 >= 0f && d3 <= 0f)
426 {
427 v = d1 / (d1 - d3);
428 result.ClosestPointOnSimplex = a + v * ab;
429 result.UsedVertices.UsedVertexA = true;
430 result.UsedVertices.UsedVertexB = true;
431 result.SetBarycentricCoordinates(1 - v, v, 0, 0);
432 return true;
433 //return a + v * ab; // barycentric coordinates (1-v,v,0)
434 }
435
436 // Check if P in vertex region outside C
437 Vector3 cp = p - c;
438 float d5 = Vector3.Dot(ab, cp);
439 float d6 = Vector3.Dot(ac, cp);
440 if (d6 >= 0f && d5 <= d6)
441 {
442 result.ClosestPointOnSimplex = c;
443 result.UsedVertices.UsedVertexC = true;
444 result.SetBarycentricCoordinates(0, 0, 1, 0);
445 return true;//c; // barycentric coordinates (0,0,1)
446 }
447
448 // Check if P in edge region of AC, if so return projection of P onto AC
449 float vb = d5 * d2 - d1 * d6;
450 if (vb <= 0f && d2 >= 0f && d6 <= 0f)
451 {
452 w = d2 / (d2 - d6);
453 result.ClosestPointOnSimplex = a + w * ac;
454 result.UsedVertices.UsedVertexA = true;
455 result.UsedVertices.UsedVertexC = true;
456 result.SetBarycentricCoordinates(1 - w, 0, w, 0);
457 return true;
458 //return a + w * ac; // barycentric coordinates (1-w,0,w)
459 }
460
461 // Check if P in edge region of BC, if so return projection of P onto BC
462 float va = d3 * d6 - d5 * d4;
463 if (va <= 0f && (d4 - d3) >= 0f && (d5 - d6) >= 0f)
464 {
465 w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
466
467 result.ClosestPointOnSimplex = b + w * (c - b);
468 result.UsedVertices.UsedVertexB = true;
469 result.UsedVertices.UsedVertexC = true;
470 result.SetBarycentricCoordinates(0, 1 - w, w, 0);
471 return true;
472 // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
473 }
474
475 // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
476 float denom = 1.0f / (va + vb + vc);
477 v = vb * denom;
478 w = vc * denom;
479
480 result.ClosestPointOnSimplex = a + ab * v + ac * w;
481 result.UsedVertices.UsedVertexA = true;
482 result.UsedVertices.UsedVertexB = true;
483 result.UsedVertices.UsedVertexC = true;
484 result.SetBarycentricCoordinates(1 - v - w, v, w, 0);
485
486 return true;
487 }
488
489 /// Test if point p and d lie on opposite sides of plane through abc
490 public int PointOutsideOfPlane(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d)
491 {
492 Vector3 normal = Vector3.Cross(b - a, c - a);
493
494 float signp = Vector3.Dot(p - a, normal); // [AP AB AC]
495 float signd = Vector3.Dot(d - a, normal); // [AD AB AC]
496
497 if (CatchDegenerateTetrahedron)
498 if (signd * signd < (1e-4f * 1e-4f)) return -1;
499
500 // Points on opposite sides if expression signs are opposite
501 return signp * signd < 0f ? 1 : 0;
502 }
503
504 public bool ClosestPtPointTetrahedron(Vector3 p, Vector3 a, Vector3 b, Vector3 c, Vector3 d,
505 ref SubSimplexClosestResult finalResult)
506 {
507 SubSimplexClosestResult tempResult = new SubSimplexClosestResult();
508
509 // Start out assuming point inside all halfspaces, so closest to itself
510 finalResult.ClosestPointOnSimplex = p;
511 finalResult.UsedVertices.Reset();
512 finalResult.UsedVertices.UsedVertexA = true;
513 finalResult.UsedVertices.UsedVertexB = true;
514 finalResult.UsedVertices.UsedVertexC = true;
515 finalResult.UsedVertices.UsedVertexD = true;
516
517 int pointOutsideABC = PointOutsideOfPlane(p, a, b, c, d);
518 int pointOutsideACD = PointOutsideOfPlane(p, a, c, d, b);
519 int pointOutsideADB = PointOutsideOfPlane(p, a, d, b, c);
520 int pointOutsideBDC = PointOutsideOfPlane(p, b, d, c, a);
521
522 if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
523 {
524 finalResult.Degenerate = true;
525 return false;
526 }
527
528 if (pointOutsideABC == 0 && pointOutsideACD == 0 && pointOutsideADB == 0 && pointOutsideBDC == 0)
529 return false;
530
531 float bestSqDist = float.MaxValue;
532 // If point outside face abc then compute closest point on abc
533 if (pointOutsideABC != 0)
534 {
535 ClosestPtPointTriangle(p, a, b, c, ref tempResult);
536 Vector3 q = tempResult.ClosestPointOnSimplex;
537
538 float sqDist = ((Vector3)(q - p)).LengthSquared();
539 // Update best closest point if (squared) distance is less than current best
540 if (sqDist < bestSqDist)
541 {
542 bestSqDist = sqDist;
543 finalResult.ClosestPointOnSimplex = q;
544 //convert result bitmask!
545 finalResult.UsedVertices.Reset();
546 finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA;
547 finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexB;
548 finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC;
549 finalResult.SetBarycentricCoordinates(
550 tempResult.BarycentricCoords[VertexA],
551 tempResult.BarycentricCoords[VertexB],
552 tempResult.BarycentricCoords[VertexC],
553 0);
554 }
555 }
556
557 // Repeat test for face acd
558 if (pointOutsideACD != 0)
559 {
560 ClosestPtPointTriangle(p, a, c, d, ref tempResult);
561 Vector3 q = tempResult.ClosestPointOnSimplex;
562 //convert result bitmask!
563
564 float sqDist = ((Vector3)(q - p)).LengthSquared();
565 if (sqDist < bestSqDist)
566 {
567 bestSqDist = sqDist;
568 finalResult.ClosestPointOnSimplex = q;
569 finalResult.UsedVertices.Reset();
570 finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA;
571 finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexB;
572 finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexC;
573 finalResult.SetBarycentricCoordinates(
574 tempResult.BarycentricCoords[VertexA],
575 0,
576 tempResult.BarycentricCoords[VertexB],
577 tempResult.BarycentricCoords[VertexC]);
578 }
579 }
580 // Repeat test for face adb
581
582 if (pointOutsideADB != 0)
583 {
584 ClosestPtPointTriangle(p, a, d, b, ref tempResult);
585 Vector3 q = tempResult.ClosestPointOnSimplex;
586 //convert result bitmask!
587
588 float sqDist = ((Vector3)(q - p)).LengthSquared();
589 if (sqDist < bestSqDist)
590 {
591 bestSqDist = sqDist;
592 finalResult.ClosestPointOnSimplex = q;
593 finalResult.UsedVertices.Reset();
594 finalResult.UsedVertices.UsedVertexA = tempResult.UsedVertices.UsedVertexA;
595 finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB;
596 finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexC;
597 finalResult.SetBarycentricCoordinates(
598 tempResult.BarycentricCoords[VertexA],
599 tempResult.BarycentricCoords[VertexC],
600 0,
601 tempResult.BarycentricCoords[VertexB]);
602
603 }
604 }
605 // Repeat test for face bdc
606
607 if (pointOutsideBDC != 0)
608 {
609 ClosestPtPointTriangle(p, b, d, c, ref tempResult);
610 Vector3 q = tempResult.ClosestPointOnSimplex;
611 //convert result bitmask!
612 float sqDist = ((Vector3)(q - p)).LengthSquared();
613 if (sqDist < bestSqDist)
614 {
615 bestSqDist = sqDist;
616 finalResult.ClosestPointOnSimplex = q;
617 finalResult.UsedVertices.Reset();
618 finalResult.UsedVertices.UsedVertexB = tempResult.UsedVertices.UsedVertexA;
619 finalResult.UsedVertices.UsedVertexD = tempResult.UsedVertices.UsedVertexB;
620 finalResult.UsedVertices.UsedVertexC = tempResult.UsedVertices.UsedVertexC;
621
622 finalResult.SetBarycentricCoordinates(
623 0,
624 tempResult.BarycentricCoords[VertexA],
625 tempResult.BarycentricCoords[VertexC],
626 tempResult.BarycentricCoords[VertexB]);
627 }
628 }
629
630 //help! we ended up full !
631
632 if (finalResult.UsedVertices.UsedVertexA &&
633 finalResult.UsedVertices.UsedVertexB &&
634 finalResult.UsedVertices.UsedVertexC &&
635 finalResult.UsedVertices.UsedVertexD)
636 {
637 return true;
638 }
639
640 return true;
641 }
642 }
643}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs
new file mode 100644
index 0000000..e4a9ae2
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactConstraint.cs
@@ -0,0 +1,488 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX.Dynamics
28{
29 public delegate float ContactSolverFunc (RigidBody bodyA, RigidBody bodyB, ManifoldPoint contactPoint, ContactSolverInfo info);
30
31 public enum ContactSolverType
32 {
33 Default = 0,
34 TypeA,
35 TypeB,
36 User,
37 MaxContactSolverType,
38 }
39
40 public class ConstraintPersistentData
41 {
42 // total applied impulse during most recent frame
43 private float _appliedImpulse;
44 private float _previousAppliedImpulse;
45 private float _accumulatedTangentImpulse0;
46 private float _accumulatedTangentImpulse1;
47
48 private float _jacDiagABInv;
49 private float _jacDiagABInvTangentA;
50 private float _jacDiagABInvTangentB;
51 private int _persistentLifeTime;
52 private float _restitution;
53 private float _friction;
54 private float _penetration;
55 private Vector3 _frictionWorldTangentialA;
56 private Vector3 _frictionWorldTangentialB;
57
58 private Vector3 _frictionAngularComponent0A;
59 private Vector3 _frictionAngularComponent0B;
60 private Vector3 _frictionAngularComponent1A;
61 private Vector3 _frictionAngularComponent1B;
62
63 //some data doesn't need to be persistent over frames: todo: clean/reuse this
64 private Vector3 _angularComponentA;
65 private Vector3 _angularComponentB;
66
67 private ContactSolverFunc _contactSolverFunc;
68 private ContactSolverFunc _frictionSolverFunc;
69
70 public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } }
71 public float PreviousAppliedImpulse { get { return _previousAppliedImpulse; } set { _previousAppliedImpulse = value; } }
72 public float AccumulatedTangentImpulseA { get { return _accumulatedTangentImpulse0; } set { _accumulatedTangentImpulse0 = value; } }
73 public float AccumulatedTangentImpulseB { get { return _accumulatedTangentImpulse1; } set { _accumulatedTangentImpulse1 = value; } }
74
75 public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } }
76 public float JacDiagABInvTangentA { get { return _jacDiagABInvTangentA; } set { _jacDiagABInvTangentA = value; } }
77 public float JacDiagABInvTangentB { get { return _jacDiagABInvTangentB; } set { _jacDiagABInvTangentB = value; } }
78 public int PersistentLifeTime { get { return _persistentLifeTime; } set { _persistentLifeTime = value; } }
79 public float Restitution { get { return _restitution; } set { _restitution = value; } }
80 public float Friction { get { return _friction; } set { _friction = value; } }
81 public float Penetration { get { return _penetration; } set { _penetration = value; } }
82 public Vector3 FrictionWorldTangentialA { get { return _frictionWorldTangentialA; } set { _frictionWorldTangentialA = value; } }
83 public Vector3 FrictionWorldTangentialB { get { return _frictionWorldTangentialB; } set { _frictionWorldTangentialB = value; } }
84
85 public Vector3 FrictionAngularComponent0A { get { return _frictionAngularComponent0A; } set { _frictionAngularComponent0A = value; } }
86 public Vector3 FrictionAngularComponent0B { get { return _frictionAngularComponent0B; } set { _frictionAngularComponent0B = value; } }
87 public Vector3 FrictionAngularComponent1A { get { return _frictionAngularComponent1A; } set { _frictionAngularComponent1A = value; } }
88 public Vector3 FrictionAngularComponent1B { get { return _frictionAngularComponent1B; } set { _frictionAngularComponent1B = value; } }
89
90 public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } }
91 public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } }
92
93 public ContactSolverFunc ContactSolverFunc { get { return _contactSolverFunc; } set { _contactSolverFunc = value; } }
94 public ContactSolverFunc FrictionSolverFunc { get { return _frictionSolverFunc; } set { _frictionSolverFunc = value; } }
95 }
96
97 public static class ContactConstraint
98 {
99 private const int UseInternalApplyImpulse = 1;
100
101 /// <summary>
102 /// bilateral constraint between two dynamic objects
103 /// positive distance = separation, negative distance = penetration
104 /// </summary>
105 /// <param name="body1"></param>
106 /// <param name="pos1"></param>
107 /// <param name="body2"></param>
108 /// <param name="pos2"></param>
109 /// <param name="distance"></param>
110 /// <param name="normal"></param>
111 /// <param name="impulse"></param>
112 /// <param name="timeStep"></param>
113 public static void ResolveSingleBilateral(RigidBody bodyA, Vector3 posA,
114 RigidBody bodyB, Vector3 posB,
115 float distance, Vector3 normal, out float impulse, float timeStep)
116 {
117 float normalLenSqr = normal.LengthSquared();
118
119 if (Math.Abs(normalLenSqr) >= 1.1f)
120 throw new BulletException();
121
122 /*if (normalLenSqr > 1.1f)
123 {
124 impulse = 0f;
125 return;
126 }*/
127 Vector3 rel_pos1 = posA - bodyA.CenterOfMassPosition;
128 Vector3 rel_pos2 = posB - bodyB.CenterOfMassPosition;
129 //this jacobian entry could be re-used for all iterations
130
131 Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
132 Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
133 Vector3 vel = vel1 - vel2;
134
135
136 JacobianEntry jac = new JacobianEntry(Matrix.Transpose(bodyA.CenterOfMassTransform),
137 Matrix.Transpose(bodyB.CenterOfMassTransform),
138 rel_pos1, rel_pos2, normal, bodyA.InvInertiaDiagLocal, bodyA.InverseMass,
139 bodyB.InvInertiaDiagLocal, bodyB.InverseMass);
140
141 float jacDiagAB = jac.Diagonal;
142 float jacDiagABInv = 1f / jacDiagAB;
143
144 float rel_vel = jac.GetRelativeVelocity(
145 bodyA.LinearVelocity,
146 Vector3.TransformNormal(bodyA.AngularVelocity, Matrix.Transpose(bodyA.CenterOfMassTransform)),
147 bodyB.LinearVelocity,
148 Vector3.TransformNormal(bodyB.AngularVelocity, Matrix.Transpose(bodyB.CenterOfMassTransform)));
149 float a;
150 a = jacDiagABInv;
151
152
153 rel_vel = Vector3.Dot(normal, vel);
154
155 float contactDamping = 0.2f;
156
157 float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv;
158 impulse = velocityImpulse;
159 }
160
161
162 /// <summary>
163 /// contact constraint resolution:
164 /// calculate and apply impulse to satisfy non-penetration and non-negative relative velocity constraint
165 /// positive distance = separation, negative distance = penetration
166 /// </summary>
167 /// <param name="body1"></param>
168 /// <param name="body2"></param>
169 /// <param name="contactPoint"></param>
170 /// <param name="info"></param>
171 /// <returns></returns>
172 public static float ResolveSingleCollision(RigidBody bodyA, RigidBody bodyB,
173 ManifoldPoint contactPoint, ContactSolverInfo solverInfo)
174 {
175 Vector3 pos1 = contactPoint.PositionWorldOnA;
176 Vector3 pos2 = contactPoint.PositionWorldOnB;
177
178
179 // printf("distance=%f\n",distance);
180
181 Vector3 normal = contactPoint.NormalWorldOnB;
182
183 Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition;
184 Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition;
185
186 Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
187 Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
188 Vector3 vel = vel1 - vel2;
189 float rel_vel;
190 rel_vel = Vector3.Dot(normal, vel);
191
192
193 float Kfps = 1f / solverInfo.TimeStep;
194
195 //float damping = solverInfo.m_damping;
196 float Kerp = solverInfo.Erp;
197
198 float Kcor = Kerp * Kfps;
199
200 //printf("dist=%f\n",distance);
201
202 ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
203 if (cpd == null)
204 throw new BulletException();
205
206 float distance = cpd.Penetration;//contactPoint.getDistance();
207
208
209 //distance = 0.f;
210 float positionalError = Kcor * -distance;
211 //jacDiagABInv;
212 float velocityError = cpd.Restitution - rel_vel;// * damping;
213
214
215 float penetrationImpulse = positionalError * cpd.JacDiagABInv;
216 float velocityImpulse = velocityError * cpd.JacDiagABInv;
217 float normalImpulse = penetrationImpulse + velocityImpulse;
218
219 // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
220 float oldNormalImpulse = cpd.AppliedImpulse;
221 float sum = oldNormalImpulse + normalImpulse;
222 cpd.AppliedImpulse = 0f > sum ? 0f : sum;
223
224 normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;
225
226 if (bodyA.InverseMass != 0)
227 {
228 bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);
229 }
230 if (bodyB.InverseMass != 0)
231 {
232 bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);
233 }
234
235 /*body1.applyImpulse(normal * (normalImpulse), rel_pos1);
236 body2.applyImpulse(-normal * (normalImpulse), rel_pos2);*/
237
238 return normalImpulse;
239 }
240
241 public static float ResolveSingleFriction(RigidBody bodyA, RigidBody bodyB,
242 ManifoldPoint contactPoint, ContactSolverInfo solverInfo)
243 {
244
245 Vector3 pos1 = contactPoint.PositionWorldOnA;
246 Vector3 pos2 = contactPoint.PositionWorldOnB;
247
248 Vector3 rel_pos1 = pos1 - bodyA.CenterOfMassPosition;
249 Vector3 rel_pos2 = pos2 - bodyB.CenterOfMassPosition;
250
251 ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
252 if (cpd == null)
253 throw new BulletException();
254
255 float combinedFriction = cpd.Friction;
256
257 float limit = cpd.AppliedImpulse * combinedFriction;
258
259 //friction
260 if (cpd.AppliedImpulse > 0)
261 {
262 //apply friction in the 2 tangential directions
263
264 // 1st tangent
265 Vector3 vel1 = bodyA.GetVelocityInLocalPoint(rel_pos1);
266 Vector3 vel2 = bodyB.GetVelocityInLocalPoint(rel_pos2);
267 Vector3 vel = vel1 - vel2;
268
269 float j1, j2;
270
271 {
272
273 float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);
274
275 // calculate j that moves us to zero relative velocity
276 j1 = -vrel * cpd.JacDiagABInvTangentA;
277 float oldTangentImpulse = cpd.AccumulatedTangentImpulseA;
278 cpd.AccumulatedTangentImpulseA = oldTangentImpulse + j1;
279 float atia = cpd.AccumulatedTangentImpulseA;
280 MathHelper.SetMin(ref atia, limit);
281 MathHelper.SetMax(ref atia, -limit);
282 cpd.AccumulatedTangentImpulseA = atia;
283 j1 = cpd.AccumulatedTangentImpulseA - oldTangentImpulse;
284
285 }
286 {
287 // 2nd tangent
288
289 float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);
290
291 // calculate j that moves us to zero relative velocity
292 j2 = -vrel * cpd.JacDiagABInvTangentB;
293 float oldTangentImpulse = cpd.AccumulatedTangentImpulseB;
294 cpd.AccumulatedTangentImpulseB = oldTangentImpulse + j2;
295 float atib = cpd.AccumulatedTangentImpulseB;
296 MathHelper.SetMin(ref atib, limit);
297 MathHelper.SetMax(ref atib, -limit);
298 cpd.AccumulatedTangentImpulseB = atib;
299 j2 = cpd.AccumulatedTangentImpulseB - oldTangentImpulse;
300 }
301
302 if (bodyA.InverseMass != 0)
303 {
304 bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyA.InverseMass, cpd.FrictionAngularComponent0A, j1);
305 bodyA.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyA.InverseMass, cpd.FrictionAngularComponent1A, j2);
306 }
307 if (bodyB.InverseMass != 0)
308 {
309 bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialA * bodyB.InverseMass, cpd.FrictionAngularComponent0B, -j1);
310 bodyB.InternalApplyImpulse(cpd.FrictionWorldTangentialB * bodyB.InverseMass, cpd.FrictionAngularComponent1B, -j2);
311 }
312
313 }
314 return cpd.AppliedImpulse;
315 }
316
317 public static float ResolveSingleFrictionOriginal(
318 RigidBody bodyA,
319 RigidBody bodyB,
320 ManifoldPoint contactPoint,
321 ContactSolverInfo solverInfo)
322 {
323 Vector3 posA = contactPoint.PositionWorldOnA;
324 Vector3 posB = contactPoint.PositionWorldOnB;
325
326 Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
327 Vector3 relPosB = posB - bodyB.CenterOfMassPosition;
328
329 ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
330 if (cpd == null)
331 throw new BulletException();
332
333 float combinedFriction = cpd.Friction;
334
335 float limit = cpd.AppliedImpulse * combinedFriction;
336 //if (contactPoint.m_appliedImpulse>0.f)
337 //friction
338 {
339 //apply friction in the 2 tangential directions
340
341 {
342 // 1st tangent
343 Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
344 Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
345 Vector3 vel = velA - velB;
346
347 float vrel = Vector3.Dot(cpd.FrictionWorldTangentialA, vel);
348
349 // calculate j that moves us to zero relative velocity
350 float j = -vrel * cpd.JacDiagABInvTangentA;
351 float total = cpd.AccumulatedTangentImpulseA + j;
352 if (limit < total)
353 total = limit;
354 if (total < -limit)
355 total = -limit;
356 j = total - cpd.AccumulatedTangentImpulseA;
357 cpd.AccumulatedTangentImpulseA = total;
358 bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialA, relPosA);
359 bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialA, relPosB);
360 }
361
362
363 {
364 // 2nd tangent
365 Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
366 Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
367 Vector3 vel = velA - velB;
368
369 float vrel = Vector3.Dot(cpd.FrictionWorldTangentialB, vel);
370
371 // calculate j that moves us to zero relative velocity
372 float j = -vrel * cpd.JacDiagABInvTangentB;
373 float total = cpd.AccumulatedTangentImpulseB + j;
374 if (limit < total)
375 total = limit;
376 if (total < -limit)
377 total = -limit;
378 j = total - cpd.AccumulatedTangentImpulseB;
379 cpd.AccumulatedTangentImpulseB = total;
380 bodyA.ApplyImpulse(j * cpd.FrictionWorldTangentialB, relPosA);
381 bodyB.ApplyImpulse(j * -cpd.FrictionWorldTangentialB, relPosB);
382 }
383 }
384
385 return cpd.AppliedImpulse;
386 }
387
388 //velocity + friction
389 //response between two dynamic objects with friction
390 public static float ResolveSingleCollisionCombined(
391 RigidBody bodyA,
392 RigidBody bodyB,
393 ManifoldPoint contactPoint,
394 ContactSolverInfo solverInfo)
395 {
396
397 Vector3 posA = contactPoint.PositionWorldOnA;
398 Vector3 posB = contactPoint.PositionWorldOnB;
399 Vector3 normal = contactPoint.NormalWorldOnB;
400
401 Vector3 relPosA = posA - bodyA.CenterOfMassPosition;
402 Vector3 relPosB = posB - bodyB.CenterOfMassPosition;
403
404 Vector3 velA = bodyA.GetVelocityInLocalPoint(relPosA);
405 Vector3 velB = bodyB.GetVelocityInLocalPoint(relPosB);
406 Vector3 vel = velA - velB;
407 float relVel;
408 relVel = Vector3.Dot(normal, vel);
409
410 float Kfps = 1f / solverInfo.TimeStep;
411
412 //float damping = solverInfo.m_damping;
413 float Kerp = solverInfo.Erp;
414 float Kcor = Kerp * Kfps;
415
416 ConstraintPersistentData cpd = contactPoint.UserPersistentData as ConstraintPersistentData;
417 if (cpd == null)
418 throw new BulletException();
419
420 float distance = cpd.Penetration;
421 float positionalError = Kcor * -distance;
422 float velocityError = cpd.Restitution - relVel;// * damping;
423
424 float penetrationImpulse = positionalError * cpd.JacDiagABInv;
425
426 float velocityImpulse = velocityError * cpd.JacDiagABInv;
427
428 float normalImpulse = penetrationImpulse + velocityImpulse;
429
430 // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
431 float oldNormalImpulse = cpd.AppliedImpulse;
432 float sum = oldNormalImpulse + normalImpulse;
433 cpd.AppliedImpulse = 0 > sum ? 0 : sum;
434
435 normalImpulse = cpd.AppliedImpulse - oldNormalImpulse;
436
437 if (bodyA.InverseMass != 0)
438 {
439 bodyA.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyA.InverseMass, cpd.AngularComponentA, normalImpulse);
440 }
441 if (bodyB.InverseMass != 0)
442 {
443 bodyB.InternalApplyImpulse(contactPoint.NormalWorldOnB * bodyB.InverseMass, cpd.AngularComponentB, -normalImpulse);
444 }
445
446 {
447 //friction
448 Vector3 vel12 = bodyA.GetVelocityInLocalPoint(relPosA);
449 Vector3 vel22 = bodyB.GetVelocityInLocalPoint(relPosB);
450 Vector3 vel3 = vel12 - vel22;
451
452 relVel = Vector3.Dot(normal, vel3);
453
454
455 Vector3 latVel = vel3 - normal * relVel;
456 float lat_rel_vel = latVel.Length();
457
458 float combinedFriction = cpd.Friction;
459
460 if (cpd.AppliedImpulse > 0)
461 if (lat_rel_vel > float.Epsilon)
462 {
463 latVel /= lat_rel_vel;
464 Vector3 temp1 = Vector3.TransformNormal(Vector3.Cross(relPosA, latVel), bodyA.InvInertiaTensorWorld);
465 Vector3 temp2 = Vector3.TransformNormal(Vector3.Cross(relPosB, latVel), bodyB.InvInertiaTensorWorld);
466 float friction_impulse = lat_rel_vel /
467 (bodyA.InverseMass + bodyB.InverseMass + Vector3.Dot(latVel, Vector3.Cross(temp1, relPosA) + Vector3.Cross(temp2, relPosB)));
468 float normal_impulse = cpd.AppliedImpulse * combinedFriction;
469
470 MathHelper.SetMin(ref friction_impulse, normal_impulse);
471 MathHelper.SetMin(ref friction_impulse, -normal_impulse);
472 bodyA.ApplyImpulse(latVel * -friction_impulse, relPosA);
473 bodyB.ApplyImpulse(latVel * friction_impulse, relPosB);
474 }
475 }
476 return normalImpulse;
477 }
478
479 public static float ResolveSingleFrictionEmpty(
480 RigidBody bodyA,
481 RigidBody bodyB,
482 ManifoldPoint contactPoint,
483 ContactSolverInfo solverInfo)
484 {
485 return 0;
486 }
487 }
488}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs
new file mode 100644
index 0000000..c1763df
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/ContactSolverInfo.cs
@@ -0,0 +1,62 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26namespace XnaDevRu.BulletX.Dynamics
27{
28 public class ContactSolverInfo
29 {
30 private float _tau;
31 private float _damping;
32 private float _friction;
33 private float _timeStep;
34 private float _restitution;
35 private int _numIterations;
36 private float _maxErrorReduction;
37 private float _sor;
38 private float _erp;
39
40 public ContactSolverInfo()
41 {
42 _tau = 0.6f;
43 _damping = 1.0f;
44 _friction = 0.3f;
45 _restitution = 0f;
46 _maxErrorReduction = 20f;
47 _numIterations = 10;
48 _erp = 0.4f;
49 _sor = 1.3f;
50 }
51
52 public float Tau { get { return _tau; } set { _tau = value; } }
53 public float Damping { get { return _damping; } set { _damping = value; } }
54 public float Friction { get { return _friction; } set { _friction = value; } }
55 public float TimeStep { get { return _timeStep; } set { _timeStep = value; } }
56 public float Restitution { get { return _restitution; } set { _restitution = value; } }
57 public int IterationsCount { get { return _numIterations; } set { _numIterations = value; } }
58 public float MaxErrorReduction { get { return _maxErrorReduction; } set { _maxErrorReduction = value; } }
59 public float Sor { get { return _sor; } set { _sor = value; } }
60 public float Erp { get { return _erp; } set { _erp = value; } }
61 }
62}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs
new file mode 100644
index 0000000..479c863
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Generic6DofConstraint.cs
@@ -0,0 +1,440 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX.Dynamics
28{
29 /// <summary>
30 /// Generic6DofConstraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
31 /// Generic6DofConstraint can leave any of the 6 degree of freedom 'free' or 'locked'
32 /// Work in progress (is still a Hinge actually)
33 /// </summary>
34 public class Generic6DofConstraint : TypedConstraint
35 {
36 private static readonly float[] _sign = { 1.0f, -1.0f, 1.0f };
37 private static readonly int[] _axisA = { 1, 0, 0 };
38 private static readonly int[] _axisB = { 2, 2, 1 };
39
40 private JacobianEntry[] _jacLinear = new JacobianEntry[3]; // 3 orthogonal linear constraints
41 private JacobianEntry[] _jacAng = new JacobianEntry[3]; // 3 orthogonal angular constraints
42
43 private Matrix _frameInA; // the constraint space w.r.t body A
44 private Matrix _frameInB; // the constraint space w.r.t body B
45
46 private float[] _lowerLimit = new float[6]; // the constraint lower limits
47 private float[] _upperLimit = new float[6]; // the constraint upper limits
48
49 private float[] _accumulatedImpulse = new float[6];
50
51 public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, Matrix frameInA, Matrix frameInB)
52 : base(rbA, rbB)
53 {
54 _frameInA = frameInA;
55 _frameInB = frameInB;
56 //free means upper < lower,
57 //locked means upper == lower
58 //limited means upper > lower
59 //so start all locked
60 for (int i = 0; i < 6; ++i)
61 {
62 _lowerLimit[i] = 0.0f;
63 _upperLimit[i] = 0.0f;
64 _accumulatedImpulse[i] = 0.0f;
65 }
66 }
67
68 public Generic6DofConstraint() { }
69
70 public void UpdateRHS(float timeStep) { }
71
72 public float ComputeAngle(int axis)
73 {
74 float angle = 0;
75
76 switch (axis)
77 {
78 case 0:
79 {
80 Vector3 v1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 1), RigidBodyA.CenterOfMassTransform);
81 Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform);
82 Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform);
83
84 float s = Vector3.Dot(v1, w2);
85 float c = Vector3.Dot(v1, v2);
86
87 angle = (float)Math.Atan2(s, c);
88 break;
89 }
90 case 1:
91 {
92 Vector3 w1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 2), RigidBodyA.CenterOfMassTransform);
93 Vector3 w2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 2), RigidBodyB.CenterOfMassTransform);
94 Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform);
95
96 float s = Vector3.Dot(w1, u2);
97 float c = Vector3.Dot(w1, w2);
98
99 angle = (float)Math.Atan2(s, c);
100 break;
101 }
102 case 2:
103 {
104 Vector3 u1 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, 0), RigidBodyA.CenterOfMassTransform);
105 Vector3 u2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 0), RigidBodyB.CenterOfMassTransform);
106 Vector3 v2 = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, 1), RigidBodyB.CenterOfMassTransform);
107
108 float s = Vector3.Dot(u1, v2);
109 float c = Vector3.Dot(u1, u2);
110
111 angle = (float)Math.Atan2(s, c);
112 break;
113 }
114 default: BulletDebug.Assert(false); break;
115 }
116
117 return angle;
118 }
119
120 public void SetLinearLowerLimit(Vector3 linearLower)
121 {
122 _lowerLimit[0] = linearLower.X;
123 _lowerLimit[1] = linearLower.Y;
124 _lowerLimit[2] = linearLower.Z;
125 }
126
127 public void SetLinearUpperLimit(Vector3 linearUpper)
128 {
129 _upperLimit[0] = linearUpper.X;
130 _upperLimit[1] = linearUpper.Y;
131 _upperLimit[2] = linearUpper.Z;
132 }
133
134 public void SetAngularLowerLimit(Vector3 angularLower)
135 {
136 _lowerLimit[3] = angularLower.X;
137 _lowerLimit[4] = angularLower.Y;
138 _lowerLimit[5] = angularLower.Z;
139 }
140
141 public void SetAngularUpperLimit(Vector3 angularUpper)
142 {
143 _upperLimit[3] = angularUpper.X;
144 _upperLimit[4] = angularUpper.Y;
145 _upperLimit[5] = angularUpper.Z;
146 }
147
148 //first 3 are linear, next 3 are angular
149 public void SetLimit(int axis, float lo, float hi)
150 {
151 _lowerLimit[axis] = lo;
152 _upperLimit[axis] = hi;
153 }
154
155 //free means upper < lower,
156 //locked means upper == lower
157 //limited means upper > lower
158 //limitIndex: first 3 are linear, next 3 are angular
159 public bool IsLimited(int limitIndex)
160 {
161 return (_upperLimit[limitIndex] >= _lowerLimit[limitIndex]);
162 }
163
164 public override void BuildJacobian()
165 {
166 Vector3 localNormalInA = new Vector3(0, 0, 0);
167
168 Vector3 pivotInA = _frameInA.Translation;
169 Vector3 pivotInB = _frameInB.Translation;
170
171 Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform);
172 Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform);
173
174 Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
175 Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;
176
177 //linear part
178 for (int i = 0; i < 3; i++)
179 {
180 if (IsLimited(i))
181 {
182 if (i == 0)
183 localNormalInA = new Vector3(1, 0, 0);
184 else if (i == 1)
185 localNormalInA = new Vector3(0, 1, 0);
186 else
187 localNormalInA = new Vector3(0, 0, 1);
188
189 Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform);
190
191 // Create linear atom
192 _jacLinear[i] = new JacobianEntry(
193 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
194 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
195 MathHelper.Transform(pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition,
196 MathHelper.Transform(pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition,
197 normalWorld,
198 RigidBodyA.InvInertiaDiagLocal,
199 RigidBodyA.InverseMass,
200 RigidBodyB.InvInertiaDiagLocal,
201 RigidBodyB.InverseMass);
202
203 //optionally disable warmstarting
204 _accumulatedImpulse[i] = 0f;
205
206 // Apply accumulated impulse
207 Vector3 impulse_vector = _accumulatedImpulse[i] * normalWorld;
208
209 RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1);
210 RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2);
211 }
212 }
213
214 // angular part
215 for (int i = 0; i < 3; i++)
216 {
217 if (IsLimited(i + 3))
218 {
219 Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform);
220 Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform);
221
222 Vector3 axis = _sign[i] * Vector3.Cross(axisA, axisB);
223
224 // Create angular atom
225 _jacAng[i] = new JacobianEntry(axis,
226 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
227 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
228 RigidBodyA.InvInertiaDiagLocal,
229 RigidBodyB.InvInertiaDiagLocal);
230
231 _accumulatedImpulse[i + 3] = 0f;
232
233 // Apply accumulated impulse
234 Vector3 impulse_vector = _accumulatedImpulse[i + 3] * axis;
235
236 RigidBodyA.ApplyTorqueImpulse(impulse_vector);
237 RigidBodyB.ApplyTorqueImpulse(-impulse_vector);
238 }
239 }
240 }
241
242 public override void SolveConstraint(float timeStep)
243 {
244 float tau = 0.1f;
245 float damping = 1.0f;
246
247 Vector3 pivotAInW = MathHelper.Transform(_frameInA.Translation, RigidBodyA.CenterOfMassTransform);
248 Vector3 pivotBInW = MathHelper.Transform(_frameInB.Translation, RigidBodyB.CenterOfMassTransform);
249
250 Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
251 Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;
252
253 Vector3 localNormalInA = new Vector3();
254
255 // linear
256 for (int i = 0; i < 3; i++)
257 {
258 if (IsLimited(i))
259 {
260 Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform));
261 Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform));
262
263 if (i == 0)
264 localNormalInA = new Vector3(1, 0, 0);
265 else if (i == 1)
266 localNormalInA = new Vector3(0, 1, 0);
267 else
268 localNormalInA = new Vector3(0, 0, 1);
269
270 Vector3 normalWorld = MathHelper.TransformNormal(localNormalInA, RigidBodyA.CenterOfMassTransform);
271
272 float jacDiagABInv = 1f / _jacLinear[i].Diagonal;
273
274 //velocity error (first order error)
275 float rel_vel = _jacLinear[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA,
276 RigidBodyB.LinearVelocity, angvelB);
277
278 //positional error (zeroth order error)
279 float depth = -Vector3.Dot(pivotAInW - pivotBInW, normalWorld);
280 float lo = -1e30f;
281 float hi = 1e30f;
282
283 //handle the limits
284 if (_lowerLimit[i] < _upperLimit[i])
285 {
286 if (depth > _upperLimit[i])
287 {
288 depth -= _upperLimit[i];
289 lo = 0f;
290 }
291 else
292 {
293 if (depth < _lowerLimit[i])
294 {
295 depth -= _lowerLimit[i];
296 hi = 0f;
297 }
298 else
299 {
300 continue;
301 }
302 }
303 }
304
305 float normalImpulse = (tau * depth / timeStep - damping * rel_vel) * jacDiagABInv;
306 float oldNormalImpulse = _accumulatedImpulse[i];
307 float sum = oldNormalImpulse + normalImpulse;
308 _accumulatedImpulse[i] = sum > hi ? 0f : sum < lo ? 0f : sum;
309 normalImpulse = _accumulatedImpulse[i] - oldNormalImpulse;
310
311 Vector3 impulse_vector = normalWorld * normalImpulse;
312 RigidBodyA.ApplyImpulse(impulse_vector, rel_pos1);
313 RigidBodyB.ApplyImpulse(-impulse_vector, rel_pos2);
314 }
315 }
316
317 Vector3 axis;
318 float angle;
319 Matrix frameAWorld = RigidBodyA.CenterOfMassTransform * _frameInA;
320 Matrix frameBWorld = RigidBodyB.CenterOfMassTransform * _frameInB;
321
322 TransformUtil.CalculateDiffAxisAngle(frameAWorld, frameBWorld, out axis, out angle);
323 Quaternion diff = new Quaternion(axis, angle);
324 Matrix diffMat = Matrix.CreateFromQuaternion(diff);
325 Vector3 xyz;
326 // this is not perfect, we can first check which axis are limited, and choose a more appropriate order
327 MatrixToEulerXYZ(diffMat, out xyz);
328
329 // angular
330 for (int i = 0; i < 3; i++)
331 {
332 if (IsLimited(i + 3))
333 {
334 Vector3 angvelA = MathHelper.TransformNormal(RigidBodyA.AngularVelocity, MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform));
335 Vector3 angvelB = MathHelper.TransformNormal(RigidBodyB.AngularVelocity, MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform));
336
337 float jacDiagABInv = 1f / _jacAng[i].Diagonal;
338
339 //velocity error (first order error)
340 float rel_vel = _jacAng[i].GetRelativeVelocity(RigidBodyA.LinearVelocity, angvelA,
341 RigidBodyB.LinearVelocity, angvelB);
342
343 //positional error (zeroth order error)
344 Vector3 axisA = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInA, _axisA[i] + 1), RigidBodyA.CenterOfMassTransform);
345 Vector3 axisB = MathHelper.TransformNormal(MathHelper.GetColumn(_frameInB, _axisB[i] + 1), RigidBodyB.CenterOfMassTransform);
346
347 float rel_pos = _sign[i] * Vector3.Dot(axisA, axisB);
348
349 float lo = -1e30f;
350 float hi = 1e30f;
351
352 //handle the twist limit
353 if (_lowerLimit[i + 3] < _upperLimit[i + 3])
354 {
355 //clamp the values
356 float loLimit = _upperLimit[i + 3] > -3.1415 ? _lowerLimit[i + 3] : -1e30f;
357 float hiLimit = _upperLimit[i + 3] < 3.1415 ? _upperLimit[i + 3] : 1e30f;
358
359 float projAngle;
360
361 if (i == 0)
362 projAngle = -2f * xyz.Z;
363 else if (i == 1)
364 projAngle = -2f * xyz.Y;
365 else
366 projAngle = -2f * xyz.Z;
367
368 if (projAngle < loLimit)
369 {
370 hi = 0f;
371 rel_pos = loLimit - projAngle;
372 }
373 else
374 {
375 if (projAngle > hiLimit)
376 {
377 lo = 0f;
378 rel_pos = (hiLimit - projAngle);
379 }
380 else
381 {
382 continue;
383 }
384 }
385 }
386
387 //impulse
388
389 float normalImpulse = -(tau * rel_pos / timeStep + damping * rel_vel) * jacDiagABInv;
390 float oldNormalImpulse = _accumulatedImpulse[i + 3];
391 float sum = oldNormalImpulse + normalImpulse;
392 _accumulatedImpulse[i + 3] = sum > hi ? 0f : sum < lo ? 0f : sum;
393 normalImpulse = _accumulatedImpulse[i + 3] - oldNormalImpulse;
394
395 Vector3 axis2 = _sign[i] * Vector3.Cross(axisA, axisB);
396 Vector3 impulse_vector = axis2 * normalImpulse;
397
398 RigidBodyA.ApplyTorqueImpulse(impulse_vector);
399 RigidBodyB.ApplyTorqueImpulse(-impulse_vector);
400 }
401 }
402 }
403
404 //MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
405 private bool MatrixToEulerXYZ(Matrix mat, out Vector3 xyz)
406 {
407 // rot = cy*cz -cy*sz sy
408 // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
409 // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
410 xyz = new Vector3();
411
412 if (MathHelper.GetElement(mat, 2) < 1.0f)
413 {
414 if (MathHelper.GetElement(mat, 2) > -1.0f)
415 {
416 xyz.X = (float)Math.Atan2(-MathHelper.GetElement(mat, 5), MathHelper.GetElement(mat, 8));
417 xyz.Y = (float)Math.Asin(MathHelper.GetElement(mat, 2));
418 xyz.Z = (float)Math.Atan2(-MathHelper.GetElement(mat, 1), MathHelper.GetElement(mat, 0));
419 return true;
420 }
421 else
422 {
423 // WARNING. Not unique. XA - ZA = -atan2(r10,r11)
424 xyz.X = -(float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4));
425 xyz.Y = -(float)Math.PI / 2;
426 xyz.Z = 0.0f;
427 return false;
428 }
429 }
430 else
431 {
432 // WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
433 xyz.X = (float)Math.Atan2(MathHelper.GetElement(mat, 3), MathHelper.GetElement(mat, 4));
434 xyz.Y = (float)Math.PI / 2;
435 xyz.Z = 0.0f;
436 return false;
437 }
438 }
439 }
440}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs
new file mode 100644
index 0000000..3e35550
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/HingeConstraint.cs
@@ -0,0 +1,246 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX.Dynamics
28{
29 /// <summary>
30 /// hinge constraint between two rigidbodies each with a pivotpoint that descibes the axis location in local space
31 /// axis defines the orientation of the hinge axis
32 /// </summary>
33 public class HingeConstraint : TypedConstraint
34 {
35 private JacobianEntry[] _jac = new JacobianEntry[3]; //3 orthogonal linear constraints
36 private JacobianEntry[] _jacAng = new JacobianEntry[3]; //2 orthogonal angular constraints + 1 for limit/motor
37
38 private Vector3 _pivotInA;
39 private Vector3 _pivotInB;
40 private Vector3 _axisInA;
41 private Vector3 _axisInB;
42
43 private bool _angularOnly;
44
45 private float _motorTargetVelocity;
46 private float _maxMotorImpulse;
47 private bool _enableAngularMotor;
48
49 public HingeConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB, Vector3 axisInA, Vector3 axisInB)
50 : base(rbA, rbB)
51 {
52 _pivotInA = pivotInA;
53 _pivotInB = pivotInB;
54 _axisInA = axisInA;
55 _axisInB = -axisInB;
56 _angularOnly = false;
57 }
58
59 public HingeConstraint(RigidBody rbA, Vector3 pivotInA, Vector3 axisInA)
60 : base(rbA)
61 {
62 _pivotInA = pivotInA;
63 _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, pivotInA);
64 _axisInA = axisInA;
65 //fixed axis in worldspace
66 _axisInB = MathHelper.TransformNormal(-axisInA, rbA.CenterOfMassTransform);
67 _angularOnly = false;
68 }
69
70 public HingeConstraint() { }
71
72 public bool AngularOnly { set { _angularOnly = value; } }
73
74 public override void BuildJacobian()
75 {
76 AppliedImpulse = 0f;
77
78 Vector3 normal = new Vector3();
79
80 if (!_angularOnly)
81 {
82 for (int i = 0; i < 3; i++)
83 {
84 MathHelper.SetElement(ref normal, i, 1);
85 _jac[i] = new JacobianEntry(
86 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
87 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
88 MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition,
89 MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition,
90 normal,
91 RigidBodyA.InvInertiaDiagLocal,
92 RigidBodyA.InverseMass,
93 RigidBodyB.InvInertiaDiagLocal,
94 RigidBodyB.InverseMass);
95 MathHelper.SetElement(ref normal, i, 0);
96 }
97 }
98
99 //calculate two perpendicular jointAxis, orthogonal to hingeAxis
100 //these two jointAxis require equal angular velocities for both bodies
101 //this is unused for now, it's a todo
102 Vector3 jointAxisALocal = new Vector3();
103 Vector3 jointAxisBLocal = new Vector3();
104 MathHelper.PlaneSpace1(_axisInA, ref jointAxisALocal, ref jointAxisBLocal);
105
106 Vector3 jointAxisA = MathHelper.TransformNormal(jointAxisALocal, RigidBodyA.CenterOfMassTransform);
107 Vector3 jointAxisB = MathHelper.TransformNormal(jointAxisBLocal, RigidBodyA.CenterOfMassTransform);
108 Vector3 hingeAxisWorld = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform);
109
110 _jacAng[0] = new JacobianEntry(jointAxisA,
111 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
112 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
113 RigidBodyA.InvInertiaDiagLocal,
114 RigidBodyB.InvInertiaDiagLocal);
115
116 _jacAng[1] = new JacobianEntry(jointAxisB,
117 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
118 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
119 RigidBodyA.InvInertiaDiagLocal,
120 RigidBodyB.InvInertiaDiagLocal);
121
122 _jacAng[2] = new JacobianEntry(hingeAxisWorld,
123 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
124 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
125 RigidBodyA.InvInertiaDiagLocal,
126 RigidBodyB.InvInertiaDiagLocal);
127 }
128
129 public override void SolveConstraint(float timeStep)
130 {
131 Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform);
132 Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform);
133
134 Vector3 normal = new Vector3(0, 0, 0);
135 float tau = 0.3f;
136 float damping = 1f;
137
138 //linear part
139 if (!_angularOnly)
140 {
141 for (int i = 0; i < 3; i++)
142 {
143 if (i == 0)
144 normal = new Vector3(1, 0, 0);
145 else if (i == 1)
146 normal = new Vector3(0, 1, 0);
147 else
148 normal = new Vector3(0, 0, 1);
149
150 float jacDiagABInv = 1f / _jac[i].Diagonal;
151
152 Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
153 Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;
154
155 Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1);
156 Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2);
157 Vector3 vel = vel1 - vel2;
158 float rel_vel;
159 rel_vel = Vector3.Dot(normal, vel);
160 //positional error (zeroth order error)
161 float depth = -Vector3.Dot(pivotAInW - pivotBInW, normal); //this is the error projected on the normal
162 float impulse = depth * tau / timeStep * jacDiagABInv - damping * rel_vel * jacDiagABInv * damping;
163 AppliedImpulse += impulse;
164 Vector3 impulse_vector = normal * impulse;
165 RigidBodyA.ApplyImpulse(impulse_vector, pivotAInW - RigidBodyA.CenterOfMassPosition);
166 RigidBodyB.ApplyImpulse(-impulse_vector, pivotBInW - RigidBodyB.CenterOfMassPosition);
167 }
168 }
169 //solve angular part
170 // get axes in world space
171 Vector3 axisA = MathHelper.TransformNormal(_axisInA, RigidBodyA.CenterOfMassTransform);
172 Vector3 axisB = MathHelper.TransformNormal(_axisInB, RigidBodyB.CenterOfMassTransform);
173
174 Vector3 angVelA = RigidBodyA.AngularVelocity;
175 Vector3 angVelB = RigidBodyB.AngularVelocity;
176 Vector3 angVelAroundHingeAxisA = axisA * Vector3.Dot(axisA, angVelA);
177 Vector3 angVelAroundHingeAxisB = axisB * Vector3.Dot(axisB, angVelB);
178
179 Vector3 angAOrthog = angVelA - angVelAroundHingeAxisA;
180 Vector3 angBOrthog = angVelB - angVelAroundHingeAxisB;
181 Vector3 velrelOrthog = angAOrthog - angBOrthog;
182
183 //solve angular velocity correction
184 float relaxation = 1f;
185 float len = velrelOrthog.Length();
186 if (len > 0.00001f)
187 {
188 Vector3 normal2 = Vector3.Normalize(velrelOrthog);
189 float denom = RigidBodyA.ComputeAngularImpulseDenominator(normal2) +
190 RigidBodyB.ComputeAngularImpulseDenominator(normal2);
191 // scale for mass and relaxation
192 velrelOrthog *= (1f / denom) * 0.9f;
193 }
194
195 //solve angular positional correction
196 Vector3 angularError = -Vector3.Cross(axisA, axisB) * (1f / timeStep);
197 float len2 = angularError.Length();
198 if (len2 > 0.00001f)
199 {
200 Vector3 normal2 = Vector3.Normalize(angularError);
201 float denom2 = RigidBodyA.ComputeAngularImpulseDenominator(normal2) +
202 RigidBodyB.ComputeAngularImpulseDenominator(normal2);
203 angularError *= (1f / denom2) * relaxation;
204 }
205
206 RigidBodyA.ApplyTorqueImpulse(-velrelOrthog + angularError);
207 RigidBodyB.ApplyTorqueImpulse(velrelOrthog - angularError);
208
209 //apply motor
210 if (_enableAngularMotor)
211 {
212 //todo: add limits too
213 Vector3 angularLimit = Vector3.Zero;
214
215 Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
216 float projRelVel = Vector3.Dot(velrel, axisA);
217
218 float desiredMotorVel = _motorTargetVelocity;
219 float motorRelvel = desiredMotorVel - projRelVel;
220
221 float denom3 = RigidBodyA.ComputeAngularImpulseDenominator(axisA) +
222 RigidBodyB.ComputeAngularImpulseDenominator(axisA);
223
224 float unclippedMotorImpulse = (1f / denom3) * motorRelvel;
225 //todo: should clip against accumulated impulse
226 float clippedMotorImpulse = unclippedMotorImpulse > _maxMotorImpulse ? _maxMotorImpulse : unclippedMotorImpulse;
227 clippedMotorImpulse = clippedMotorImpulse < -_maxMotorImpulse ? -_maxMotorImpulse : clippedMotorImpulse;
228 Vector3 motorImp = clippedMotorImpulse * axisA;
229
230 RigidBodyA.ApplyTorqueImpulse(motorImp + angularLimit);
231 RigidBodyB.ApplyTorqueImpulse(-motorImp - angularLimit);
232 }
233 }
234
235 public void EnableAngularMotor(bool enableMotor, float targetVelocity, float maxMotorImpulse)
236 {
237 _enableAngularMotor = enableMotor;
238 _motorTargetVelocity = targetVelocity;
239 _maxMotorImpulse = maxMotorImpulse;
240 }
241
242 public void UpdateRHS(float timeStep)
243 {
244 }
245 }
246}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs
new file mode 100644
index 0000000..de25225
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/IConstraintSolver.cs
@@ -0,0 +1,32 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26namespace XnaDevRu.BulletX.Dynamics
27{
28 public interface IConstraintSolver
29 {
30 float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo info, IDebugDraw debugDrawer);
31 }
32}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs
new file mode 100644
index 0000000..3b8d8d8
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/JacobianEntry.cs
@@ -0,0 +1,155 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX.Dynamics
28{
29 /// <summary>
30 /// Jacobian entry is an abstraction that allows to describe constraints
31 /// it can be used in combination with a constraint solver
32 /// Can be used to relate the effect of an impulse to the constraint error
33 /// </summary>
34 public class JacobianEntry
35 {
36 private Vector3 _linearJointAxis;
37 private Vector3 _aJ;
38 private Vector3 _bJ;
39 private Vector3 _0MinvJt;
40 private Vector3 _1MinvJt;
41 private float _adiag;
42
43 public JacobianEntry() { }
44
45 //constraint between two different rigidbodies
46 public JacobianEntry(
47 Matrix world2A,
48 Matrix world2B,
49 Vector3 relPosA, Vector3 relPosB,
50 Vector3 jointAxis,
51 Vector3 inertiaInvA,
52 float massInvA,
53 Vector3 inertiaInvB,
54 float massInvB)
55 {
56 _linearJointAxis = jointAxis;
57 _aJ = Vector3.TransformNormal(Vector3.Cross(relPosA, _linearJointAxis), world2A);
58 _bJ = Vector3.TransformNormal(Vector3.Cross(relPosB, -_linearJointAxis), world2B);
59 _0MinvJt = inertiaInvA * _aJ;
60 _1MinvJt = inertiaInvB * _bJ;
61 _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ) + massInvB + Vector3.Dot(_1MinvJt, _bJ);
62
63 if (_adiag <= 0.0f)
64 throw new BulletException();
65 }
66
67 //angular constraint between two different rigidbodies
68 public JacobianEntry(Vector3 jointAxis,
69 Matrix world2A,
70 Matrix world2B,
71 Vector3 inertiaInvA,
72 Vector3 inertiaInvB)
73 {
74 _linearJointAxis = new Vector3();
75 _aJ = Vector3.TransformNormal(jointAxis, world2A);
76 _bJ = Vector3.TransformNormal(-jointAxis, world2B);
77 _0MinvJt = inertiaInvA * _aJ;
78 _1MinvJt = inertiaInvB * _bJ;
79 _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ);
80
81 if (_adiag <= 0.0f)
82 throw new BulletException();
83 }
84
85 //angular constraint between two different rigidbodies
86 public JacobianEntry(Vector3 axisInA,
87 Vector3 axisInB,
88 Vector3 inertiaInvA,
89 Vector3 inertiaInvB)
90 {
91 _linearJointAxis = new Vector3();
92 _aJ = axisInA;
93 _bJ = -axisInB;
94 _0MinvJt = inertiaInvA * _aJ;
95 _1MinvJt = inertiaInvB * _bJ;
96 _adiag = Vector3.Dot(_0MinvJt, _aJ) + Vector3.Dot(_1MinvJt, _bJ);
97
98 if (_adiag <= 0.0f)
99 throw new BulletException();
100 }
101
102 //constraint on one rigidbody
103 public JacobianEntry(
104 Matrix world2A,
105 Vector3 rel_pos1, Vector3 rel_pos2,
106 Vector3 jointAxis,
107 Vector3 inertiaInvA,
108 float massInvA)
109 {
110 _linearJointAxis = jointAxis;
111 _aJ = Vector3.TransformNormal(Vector3.Cross(rel_pos1, jointAxis), world2A);
112 _bJ = Vector3.TransformNormal(Vector3.Cross(rel_pos2, -jointAxis), world2A);
113 _0MinvJt = inertiaInvA * _aJ;
114 _1MinvJt = new Vector3();
115 _adiag = massInvA + Vector3.Dot(_0MinvJt, _aJ);
116
117 if (_adiag <= 0.0f)
118 throw new BulletException();
119 }
120
121 public float Diagonal { get { return _adiag; } }
122
123 // for two constraints on the same rigidbody (for example vehicle friction)
124 public float GetNonDiagonal(JacobianEntry jacB, float massInvA)
125 {
126 float lin = massInvA * Vector3.Dot(_linearJointAxis, jacB._linearJointAxis);
127 float ang = Vector3.Dot(_0MinvJt, jacB._aJ);
128 return lin + ang;
129 }
130
131 // for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
132 public float GetNonDiagonal(JacobianEntry jacB, float massInvA, float massInvB)
133 {
134 Vector3 lin = _linearJointAxis * jacB._linearJointAxis;
135 Vector3 ang0 = _0MinvJt * jacB._aJ;
136 Vector3 ang1 = _1MinvJt * jacB._bJ;
137 Vector3 lin0 = massInvA * lin;
138 Vector3 lin1 = massInvB * lin;
139 Vector3 sum = ang0 + ang1 + lin0 + lin1;
140 return sum.X + sum.Y + sum.Z;
141 }
142
143 public float GetRelativeVelocity(Vector3 linvelA, Vector3 angvelA, Vector3 linvelB, Vector3 angvelB)
144 {
145 Vector3 linrel = linvelA - linvelB;
146 Vector3 angvela = angvelA * _aJ;
147 Vector3 angvelb = angvelB * _bJ;
148 linrel *= _linearJointAxis;
149 angvela += angvelb;
150 angvela += linrel;
151 float rel_vel2 = angvela.X + angvela.Y + angvela.Z;
152 return rel_vel2 + float.Epsilon;
153 }
154 }
155}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs
new file mode 100644
index 0000000..d4206ea
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Point2PointConstraint.cs
@@ -0,0 +1,157 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX.Dynamics
28{
29 public class ConstraintSetting
30 {
31 private float _tau, _damping;
32
33 public ConstraintSetting()
34 {
35 _tau = 0.3f;
36 _damping = 1.0f;
37 }
38
39 public float Damping
40 {
41 get { return _damping; }
42 set { _damping = value; }
43 }
44
45 public float Tau
46 {
47 get { return _tau; }
48 set { _tau = value; }
49 }
50 }
51
52 public class Point2PointConstraint : TypedConstraint
53 {
54 private JacobianEntry[] _jacobian;
55 private Vector3 _pivotInA, _pivotInB;
56
57 private ConstraintSetting _setting = new ConstraintSetting();
58
59 public Point2PointConstraint()
60 {
61 _jacobian = new JacobianEntry[3];
62 }
63
64 public Point2PointConstraint(RigidBody rbA, RigidBody rbB, Vector3 pivotInA, Vector3 pivotInB)
65 : base(rbA, rbB)
66 {
67 _jacobian = new JacobianEntry[3];
68
69 _pivotInA = pivotInA;
70 _pivotInB = pivotInB;
71 }
72
73 public Point2PointConstraint(RigidBody rbA, Vector3 pivotInA)
74 : base(rbA)
75 {
76 _jacobian = new JacobianEntry[3];
77
78 _pivotInA = pivotInA;
79 _pivotInB = MathHelper.MatrixToVector(rbA.CenterOfMassTransform, _pivotInA);
80 }
81
82 public ConstraintSetting Settings { get { return _setting; } set { _setting = value; } }
83
84 public Vector3 PivotInA
85 {
86 set
87 {
88 _pivotInA = value;
89 }
90 }
91
92 public Vector3 PivotInB
93 {
94 set
95 {
96 _pivotInB = value;
97 }
98 }
99
100 public override void BuildJacobian()
101 {
102 Vector3 normal = new Vector3();
103
104 for (int i = 0; i < 3; i++)
105 {
106 MathHelper.SetElement(ref normal, i, 1);
107 _jacobian[i] = new JacobianEntry(
108 MatrixOperations.Transpose(RigidBodyA.CenterOfMassTransform),
109 MatrixOperations.Transpose(RigidBodyB.CenterOfMassTransform),
110 MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform) - RigidBodyA.CenterOfMassPosition,
111 MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform) - RigidBodyB.CenterOfMassPosition,
112 normal,
113 RigidBodyA.InvInertiaDiagLocal,
114 RigidBodyA.InverseMass,
115 RigidBodyB.InvInertiaDiagLocal,
116 RigidBodyB.InverseMass
117 );
118 MathHelper.SetElement(ref normal, i, 0);
119 }
120 }
121
122 public override void SolveConstraint(float timeStep)
123 {
124 Vector3 pivotAInW = MathHelper.Transform(_pivotInA, RigidBodyA.CenterOfMassTransform);
125 Vector3 pivotBInW = MathHelper.Transform(_pivotInB, RigidBodyB.CenterOfMassTransform);
126
127 Vector3 normal = new Vector3();
128
129 for (int i = 0; i < 3; i++)
130 {
131 MathHelper.SetElement(ref normal, i, 1);
132
133 float jacDiagABInv = 1.0f / _jacobian[i].Diagonal;
134
135 Vector3 rel_pos1 = pivotAInW - RigidBodyA.CenterOfMassPosition;
136 Vector3 rel_pos2 = pivotBInW - RigidBodyB.CenterOfMassPosition;
137
138 Vector3 vel1 = RigidBodyA.GetVelocityInLocalPoint(rel_pos1);
139 Vector3 vel2 = RigidBodyB.GetVelocityInLocalPoint(rel_pos2);
140
141 Vector3 vel = vel1 - vel2;
142
143 float rel_vel = Vector3.Dot(normal, vel);
144 float depth = -Vector3.Dot((pivotAInW - pivotBInW), normal);
145
146 float impulse = depth * _setting.Tau / timeStep * jacDiagABInv - _setting.Damping * rel_vel * jacDiagABInv;
147 AppliedImpulse += impulse;
148 Vector3 impulseVector = normal * impulse;
149
150 RigidBodyA.ApplyImpulse(impulseVector, pivotAInW - RigidBodyA.CenterOfMassPosition);
151 RigidBodyB.ApplyImpulse(-impulseVector, pivotBInW - RigidBodyB.CenterOfMassPosition);
152
153 MathHelper.SetElement(ref normal, i, 0);
154 }
155 }
156 }
157} \ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs
new file mode 100644
index 0000000..4d5e93d
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SequentialImpulseConstraintSolver.cs
@@ -0,0 +1,915 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX.Dynamics
28{
29 [Flags]
30 public enum SolverMode
31 {
32 None = 0,
33 RandomizeOrder = 1,
34 FrictionSeperate = 2,
35 UseWarmstarting = 4,
36 CacheFriendly = 8,
37 }
38
39 public class SequentialImpulseConstraintSolver : IConstraintSolver
40 {
41 private static int _totalContactPoints = 0;
42
43 private SolverMode _solverMode;
44 private int _totalCpd = 0;
45 private ContactSolverFunc[,] _contactDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType];
46 private ContactSolverFunc[,] _frictionDispatch = new ContactSolverFunc[(int)ContactSolverType.MaxContactSolverType, (int)ContactSolverType.MaxContactSolverType];
47
48 private float _penetrationResolveFactor = 0.9f;
49 private List<SolverBody> _tmpSolverBodyPool = new List<SolverBody>();
50 private List<SolverConstraint> _tmpSolverConstraintPool = new List<SolverConstraint>();
51 private List<SolverConstraint> _tmpSolverFrictionConstraintPool = new List<SolverConstraint>();
52
53 private const int _sequentialImpulseMaxSolverPoints = 16384;
54 private static OrderIndex[] _order = new OrderIndex[SequentialImpulseMaxSolverPoints];
55 private static long _seed2 = 0;
56
57 public SequentialImpulseConstraintSolver()
58 {
59 _solverMode = SolverMode.RandomizeOrder | SolverMode.CacheFriendly;
60 PersistentManifold.ContactDestroyedCallback = MyContactDestroyedCallback;
61
62 //initialize default friction/contact funcs
63 int i, j;
64 for (i = 0; i < (int)ContactSolverType.MaxContactSolverType; i++)
65 for (j = 0; j < (int)ContactSolverType.MaxContactSolverType; j++)
66 {
67
68 _contactDispatch[i, j] = ContactConstraint.ResolveSingleCollision;
69 _frictionDispatch[i, j] = ContactConstraint.ResolveSingleFriction;
70 }
71 }
72
73 public SolverMode SolverMode { get { return _solverMode; } set { _solverMode = value; } }
74 public static int SequentialImpulseMaxSolverPoints { get { return _sequentialImpulseMaxSolverPoints; } }
75 protected static OrderIndex[] Order { get { return _order; } set { _order = value; } }
76 public static long RandSeed { get { return _seed2; } set { _seed2 = value; } }
77
78 ///<summary>
79 /// Advanced: Override the default contact solving function for contacts, for certain types of rigidbody
80 /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
81 ///</summary>
82 public void SetContactSolverFunc(ContactSolverFunc func, int typeA, int typeB)
83 {
84 _contactDispatch[typeA, typeB] = func;
85 }
86
87 /// <summary>
88 /// Advanced: Override the default friction solving function for contacts, for certain types of rigidbody
89 /// See btRigidBody::m_contactSolverType and btRigidBody::m_frictionSolverType
90 ///</summary>
91 public void SetFrictionSolverFunc(ContactSolverFunc func, int typeA, int typeB)
92 {
93 _frictionDispatch[typeA, typeB] = func;
94 }
95
96 protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
97 {
98 float maxImpulse = 0;
99
100 Vector3 color = new Vector3(0, 1, 0);
101 if (cp.Distance <= 0)
102 {
103 if (iter == 0)
104 if(debugDraw != null)
105 debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);
106
107 ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
108 float impulse = cpd.ContactSolverFunc(
109 bodyA, bodyB,
110 cp,
111 info);
112
113 if (maxImpulse < impulse)
114 maxImpulse = impulse;
115 }
116 return maxImpulse;
117 }
118
119 protected float Solve(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter)
120 {
121 return Solve(bodyA, bodyB, cp, info, iter, null);
122 }
123
124 protected float SolveCombinedContactFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
125 {
126 float maxImpulse = 0;
127
128 Vector3 color = new Vector3(0, 1, 0);
129 if (cp.Distance <= 0)
130 {
131 if (iter == 0)
132 if (debugDraw != null)
133 debugDraw.DrawContactPoint(cp.PositionWorldOnB, cp.NormalWorldOnB, cp.Distance, cp.LifeTime, color);
134
135 float impulse = ContactConstraint.ResolveSingleCollisionCombined(
136 bodyA, bodyB,
137 cp,
138 info);
139
140 if (maxImpulse < impulse)
141 maxImpulse = impulse;
142 }
143 return maxImpulse;
144 }
145
146 protected float SolveFriction(RigidBody bodyA, RigidBody bodyB, ManifoldPoint cp, ContactSolverInfo info, int iter, IDebugDraw debugDraw)
147 {
148 Vector3 color = new Vector3(0, 1, 0);
149
150 if (cp.Distance <= 0)
151 {
152
153 ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
154 cpd.FrictionSolverFunc(
155 bodyA, bodyB,
156 cp,
157 info);
158 }
159 return 0;
160 }
161
162 protected void PrepareConstraints(PersistentManifold manifold, ContactSolverInfo info)
163 {
164 RigidBody body0 = manifold.BodyA as RigidBody;
165 RigidBody body1 = manifold.BodyB as RigidBody;
166
167
168 //only necessary to refresh the manifold once (first iteration). The integration is done outside the loop
169 {
170 manifold.RefreshContactPoints(body0.CenterOfMassTransform, body1.CenterOfMassTransform);
171
172 int numpoints = manifold.ContactsCount;
173
174 _totalContactPoints += numpoints;
175
176 Vector3 color = new Vector3(0, 1, 0);
177 for (int i = 0; i < numpoints; i++)
178 {
179 ManifoldPoint cp = manifold.GetContactPoint(i);
180 if (cp.Distance <= 0)
181 {
182 Vector3 pos1 = cp.PositionWorldOnA;
183 Vector3 pos2 = cp.PositionWorldOnB;
184
185 Vector3 rel_pos1 = pos1 - body0.CenterOfMassPosition;
186 Vector3 rel_pos2 = pos2 - body1.CenterOfMassPosition;
187
188
189 //this jacobian entry is re-used for all iterations
190 JacobianEntry jac = new JacobianEntry(MatrixOperations.Transpose(body0.CenterOfMassTransform),
191 MatrixOperations.Transpose(body1.CenterOfMassTransform),
192 rel_pos1, rel_pos2, cp.NormalWorldOnB, body0.InvInertiaDiagLocal, body0.InverseMass,
193 body1.InvInertiaDiagLocal, body1.InverseMass);
194
195 float jacDiagAB = jac.Diagonal;
196
197 ConstraintPersistentData cpd = cp.UserPersistentData as ConstraintPersistentData;
198 if (cpd != null)
199 {
200 //might be invalid
201 cpd.PersistentLifeTime++;
202 if (cpd.PersistentLifeTime != cp.LifeTime)
203 {
204 //printf("Invalid: cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd->m_persistentLifeTime,cp.getLifeTime());
205 cpd = new ConstraintPersistentData();
206 cpd.PersistentLifeTime = cp.LifeTime;
207
208 }
209 }
210 else
211 {
212
213 cpd = new ConstraintPersistentData();
214 _totalCpd++;
215 //printf("totalCpd = %i Created Ptr %x\n",totalCpd,cpd);
216 cp.UserPersistentData = cpd;
217 cpd.PersistentLifeTime = cp.LifeTime;
218 //printf("CREATED: %x . cpd->m_persistentLifeTime = %i cp.getLifeTime() = %i\n",cpd,cpd->m_persistentLifeTime,cp.getLifeTime());
219
220 }
221 if (cpd == null)
222 throw new BulletException();
223
224 cpd.JacDiagABInv = 1f / jacDiagAB;
225
226 //Dependent on Rigidbody A and B types, fetch the contact/friction response func
227 //perhaps do a similar thing for friction/restutution combiner funcs...
228
229 cpd.FrictionSolverFunc = _frictionDispatch[(int)body0.FrictionSolverType, (int)body1.FrictionSolverType];
230 cpd.ContactSolverFunc = _contactDispatch[(int)body0.ContactSolverType, (int)body1.ContactSolverType];
231
232 Vector3 vel1 = body0.GetVelocityInLocalPoint(rel_pos1);
233 Vector3 vel2 = body1.GetVelocityInLocalPoint(rel_pos2);
234 Vector3 vel = vel1 - vel2;
235 float rel_vel;
236 rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel);
237
238 float combinedRestitution = cp.CombinedRestitution;
239
240 cpd.Penetration = cp.Distance;
241 cpd.Friction = cp.CombinedFriction;
242 cpd.Restitution = RestitutionCurve(rel_vel, combinedRestitution);
243 if (cpd.Restitution < 0f)
244 {
245 cpd.Restitution = 0.0f;
246
247 };
248
249 //restitution and penetration work in same direction so
250 //rel_vel
251
252 float penVel = -cpd.Penetration / info.TimeStep;
253
254 if (cpd.Restitution > penVel)
255 {
256 cpd.Penetration = 0;
257 }
258
259
260 float relaxation = info.Damping;
261 if ((_solverMode & SolverMode.UseWarmstarting) != 0)
262 {
263 cpd.AppliedImpulse *= relaxation;
264 }
265 else
266 {
267 cpd.AppliedImpulse = 0f;
268 }
269
270 //for friction
271 cpd.PreviousAppliedImpulse = cpd.AppliedImpulse;
272
273 //re-calculate friction direction every frame, todo: check if this is really needed
274 Vector3 fwta = cpd.FrictionWorldTangentialA;
275 Vector3 fwtb = cpd.FrictionWorldTangentialB;
276 MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref fwta, ref fwtb);
277 cpd.FrictionWorldTangentialA = fwta;
278 cpd.FrictionWorldTangentialB = fwtb;
279
280 cpd.AccumulatedTangentImpulseA = 0;
281 cpd.AccumulatedTangentImpulseB = 0;
282 float denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialA);
283 float denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialA);
284 float denom = relaxation / (denom0 + denom1);
285 cpd.JacDiagABInvTangentA = denom;
286
287
288 denom0 = body0.ComputeImpulseDenominator(pos1, cpd.FrictionWorldTangentialB);
289 denom1 = body1.ComputeImpulseDenominator(pos2, cpd.FrictionWorldTangentialB);
290 denom = relaxation / (denom0 + denom1);
291 cpd.JacDiagABInvTangentB = denom;
292
293 Vector3 totalImpulse = cp.NormalWorldOnB * cpd.AppliedImpulse;
294
295 {
296 Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
297 cpd.AngularComponentA = Vector3.TransformNormal(torqueAxis0, body0.InvInertiaTensorWorld);
298 Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);
299 cpd.AngularComponentB = Vector3.TransformNormal(torqueAxis1, body1.InvInertiaTensorWorld);
300 }
301 {
302 Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialA);
303 cpd.FrictionAngularComponent0A = Vector3.TransformNormal(ftorqueAxis0, body0.InvInertiaTensorWorld);
304 }
305 {
306 Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, cpd.FrictionWorldTangentialB);
307 cpd.FrictionAngularComponent1A = Vector3.TransformNormal(ftorqueAxis1, body0.InvInertiaTensorWorld);
308 }
309 {
310 Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialA);
311 cpd.FrictionAngularComponent0B = Vector3.TransformNormal(ftorqueAxis0, body1.InvInertiaTensorWorld);
312 }
313 {
314 Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, cpd.FrictionWorldTangentialB);
315 cpd.FrictionAngularComponent1B = Vector3.TransformNormal(ftorqueAxis1, body1.InvInertiaTensorWorld);
316 }
317
318
319 //apply previous frames impulse on both bodies
320 body0.ApplyImpulse(totalImpulse, rel_pos1);
321 body1.ApplyImpulse(-totalImpulse, rel_pos2);
322 }
323 }
324 }
325 }
326
327 private bool MyContactDestroyedCallback(object userPersistentData)
328 {
329 if (userPersistentData == null)
330 throw new BulletException();
331 ConstraintPersistentData cpd = userPersistentData as ConstraintPersistentData;
332 _totalCpd--;
333 return true;
334 }
335
336 private float RestitutionCurve(float relVel, float restitution)
337 {
338 float rest = restitution * -relVel;
339 return rest;
340 }
341
342 //velocity + friction
343 //response between two dynamic objects with friction
344 public virtual float ResolveSingleCollisionCombinedCacheFriendly(
345 SolverBody bodyA,
346 SolverBody bodyB,
347 SolverConstraint contactConstraint,
348 ContactSolverInfo solverInfo)
349 {
350 float normalImpulse = 0;
351
352 if (contactConstraint.Penetration < 0)
353 return 0;
354
355 float relVel;
356 float velADotn = Vector3.Dot(contactConstraint.ContactNormal,bodyA.LinearVelocity)
357 + Vector3.Dot(contactConstraint.RelPosACrossNormal,bodyA.AngularVelocity);
358 float velBDotn = Vector3.Dot(contactConstraint.ContactNormal,bodyB.LinearVelocity)
359 + Vector3.Dot(contactConstraint.RelPosBCrossNormal,bodyB.AngularVelocity);
360
361 relVel = velADotn - velBDotn;
362
363 float positionalError = contactConstraint.Penetration;
364 float velocityError = contactConstraint.Restitution - relVel;// * damping;
365
366 float penetrationImpulse = positionalError * contactConstraint.JacDiagABInv;
367 float velocityImpulse = velocityError * contactConstraint.JacDiagABInv;
368 normalImpulse = penetrationImpulse + velocityImpulse;
369
370 // See Erin Catto's GDC 2006 paper: Clamp the accumulated impulse
371 float oldNormalImpulse = contactConstraint.AppliedImpulse;
372 float sum = oldNormalImpulse + normalImpulse;
373 contactConstraint.AppliedImpulse = 0 > sum ? 0 : sum;
374
375 float oldVelocityImpulse = contactConstraint.AppliedVelocityImpulse;
376 float velocitySum = oldVelocityImpulse + velocityImpulse;
377 contactConstraint.AppliedVelocityImpulse = 0 > velocitySum ? 0 : velocitySum;
378
379 normalImpulse = contactConstraint.AppliedImpulse - oldNormalImpulse;
380
381 if (bodyA.InvMass != 0)
382 {
383 bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass,
384 contactConstraint.AngularComponentA, normalImpulse);
385 }
386 if (bodyB.InvMass != 0)
387 {
388 bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass,
389 contactConstraint.AngularComponentB, -normalImpulse);
390 }
391
392 return normalImpulse;
393 }
394
395 public virtual float ResolveSingleFrictionCacheFriendly(
396 SolverBody bodyA,
397 SolverBody bodyB,
398 SolverConstraint contactConstraint,
399 ContactSolverInfo solverInfo,
400 float appliedNormalImpulse)
401 {
402 float combinedFriction = contactConstraint.Friction;
403 float limit = appliedNormalImpulse * combinedFriction;
404
405 if (appliedNormalImpulse > 0)
406 //friction
407 {
408 float j1;
409 {
410 float relVel;
411 float velADotn = Vector3.Dot(contactConstraint.ContactNormal, bodyA.LinearVelocity)
412 + Vector3.Dot(contactConstraint.RelPosACrossNormal, bodyA.AngularVelocity);
413 float velBDotn = Vector3.Dot(contactConstraint.ContactNormal, bodyB.LinearVelocity)
414 + Vector3.Dot(contactConstraint.RelPosBCrossNormal, bodyB.AngularVelocity);
415 relVel = velADotn - velBDotn;
416
417 // calculate j that moves us to zero relative velocity
418 j1 = -relVel * contactConstraint.JacDiagABInv;
419 float oldTangentImpulse = contactConstraint.AppliedImpulse;
420 contactConstraint.AppliedImpulse = oldTangentImpulse + j1;
421
422 float test = contactConstraint.AppliedImpulse;
423 MathHelper.SetMin(ref test, limit);
424 MathHelper.SetMax(ref test, -limit);
425 contactConstraint.AppliedImpulse = test;
426
427 j1 = contactConstraint.AppliedImpulse - oldTangentImpulse;
428 }
429
430 if (bodyA.InvMass != 0)
431 {
432 bodyA.ApplyImpulse(contactConstraint.ContactNormal * bodyA.InvMass, contactConstraint.AngularComponentA, j1);
433 }
434 if (bodyB.InvMass != 0)
435 {
436 bodyB.ApplyImpulse(contactConstraint.ContactNormal * bodyB.InvMass, contactConstraint.AngularComponentB, -j1);
437 }
438 }
439 return 0;
440 }
441
442 public virtual float SolveGroupCacheFriendly(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer)
443 {
444 if (constraints.Count + numManifolds == 0)
445 {
446 return 0;
447 }
448
449 for (int i = 0; i < numManifolds; i++)
450 {
451 PersistentManifold manifold = manifolds[i];
452 RigidBody rbA = (RigidBody)manifold.BodyA;
453 RigidBody rbB = (RigidBody)manifold.BodyB;
454
455 manifold.RefreshContactPoints(rbA.CenterOfMassTransform, rbB.CenterOfMassTransform);
456 }
457
458 int minReservation = manifolds.Count * 2;
459
460 _tmpSolverBodyPool = new List<SolverBody>(minReservation);
461
462 for (int i = 0; i < bodies.Count; i++)
463 {
464 RigidBody rb = RigidBody.Upcast(bodies[i]);
465 if (rb != null && rb.IslandTag >= 0)
466 {
467 BulletDebug.Assert(rb.CompanionID < 0);
468 int solverBodyId = _tmpSolverBodyPool.Count;
469 SolverBody solverBody;
470 InitSolverBody(out solverBody, rb);
471 _tmpSolverBodyPool.Add(solverBody);
472 rb.CompanionID = solverBodyId;
473 }
474 }
475
476 _tmpSolverConstraintPool = new List<SolverConstraint>(minReservation);
477 _tmpSolverFrictionConstraintPool = new List<SolverConstraint>(minReservation);
478
479 for (int i = 0; i < numManifolds; i++)
480 {
481 PersistentManifold manifold = manifolds[i];
482 RigidBody rb0 = (RigidBody)manifold.BodyA;
483 RigidBody rb1 = (RigidBody)manifold.BodyB;
484
485 int solverBodyIdA = -1;
486 int solverBodyIdB = -1;
487
488 //if (i == 89)
489 // System.Diagnostics.Debugger.Break();
490
491 if (manifold.ContactsCount != 0)
492 {
493 if (rb0.IslandTag >= 0)
494 {
495 solverBodyIdA = rb0.CompanionID;
496 }
497 else
498 {
499 //create a static body
500 solverBodyIdA = _tmpSolverBodyPool.Count;
501 SolverBody solverBody;
502 InitSolverBody(out solverBody, rb0);
503 _tmpSolverBodyPool.Add(solverBody);
504 }
505
506 if (rb1.IslandTag >= 0)
507 {
508 solverBodyIdB = rb1.CompanionID;
509 }
510 else
511 {
512 //create a static body
513 solverBodyIdB = _tmpSolverBodyPool.Count;
514 SolverBody solverBody;
515 InitSolverBody(out solverBody, rb1);
516 _tmpSolverBodyPool.Add(solverBody);
517 }
518 }
519
520 if (solverBodyIdB == -1 || solverBodyIdA == -1)
521 System.Diagnostics.Debug.WriteLine(string.Format("We're in ass ! {0}", i));
522
523 for (int j = 0; j < manifold.ContactsCount; j++)
524 {
525 ManifoldPoint cp = manifold.GetContactPoint(j);
526
527 int frictionIndex = _tmpSolverConstraintPool.Count;
528
529 if (cp.Distance <= 0)
530 {
531
532 Vector3 pos1 = cp.PositionWorldOnA;
533 Vector3 pos2 = cp.PositionWorldOnB;
534
535 Vector3 rel_pos1 = pos1 - rb0.CenterOfMassPosition;
536 Vector3 rel_pos2 = pos2 - rb1.CenterOfMassPosition;
537
538 float relaxation = 1;
539 {
540 SolverConstraint solverConstraint = new SolverConstraint();
541 _tmpSolverConstraintPool.Add(solverConstraint);
542
543 solverConstraint.SolverBodyIdA = solverBodyIdA;
544 solverConstraint.SolverBodyIdB = solverBodyIdB;
545 solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Contact;
546
547 //can be optimized, the cross products are already calculated
548 float denom0 = rb0.ComputeImpulseDenominator(pos1, cp.NormalWorldOnB);
549 float denom1 = rb1.ComputeImpulseDenominator(pos2, cp.NormalWorldOnB);
550 float denom = relaxation / (denom0 + denom1);
551 solverConstraint.JacDiagABInv = denom;
552
553 solverConstraint.ContactNormal = cp.NormalWorldOnB;
554 solverConstraint.RelPosACrossNormal = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
555 solverConstraint.RelPosBCrossNormal = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);
556
557 Vector3 vel1 = rb0.GetVelocityInLocalPoint(rel_pos1);
558 Vector3 vel2 = rb1.GetVelocityInLocalPoint(rel_pos2);
559
560 Vector3 vel = vel1 - vel2;
561 float rel_vel;
562 rel_vel = Vector3.Dot(cp.NormalWorldOnB, vel);
563
564
565 solverConstraint.Penetration = cp.Distance;//btScalar(infoGlobal.m_numIterations);
566 solverConstraint.Friction = cp.CombinedFriction;
567 float rest = RestitutionCurve(rel_vel, cp.CombinedRestitution);
568 if (rest <= 0)
569 {
570 rest = 0;
571 }
572
573 float penVel = -solverConstraint.Penetration / infoGlobal.TimeStep;
574 if (rest > penVel)
575 {
576 rest = 0;
577 }
578 solverConstraint.Restitution = rest;
579
580 solverConstraint.Penetration *= -(infoGlobal.Erp / infoGlobal.TimeStep);
581
582 solverConstraint.AppliedImpulse = 0f;
583 solverConstraint.AppliedVelocityImpulse = 0f;
584
585#warning Check to see if we need Vector3.Transform
586 Vector3 torqueAxis0 = Vector3.Cross(rel_pos1, cp.NormalWorldOnB);
587 solverConstraint.AngularComponentA = Vector3.TransformNormal(torqueAxis0, rb0.InvInertiaTensorWorld);
588 Vector3 torqueAxis1 = Vector3.Cross(rel_pos2, cp.NormalWorldOnB);
589 solverConstraint.AngularComponentB = Vector3.TransformNormal(torqueAxis1, rb1.InvInertiaTensorWorld);
590 }
591 //create 2 '1d axis' constraints for 2 tangential friction directions
592
593 //re-calculate friction direction every frame, todo: check if this is really needed
594 Vector3 frictionTangential0a = new Vector3(),
595 frictionTangential1b = new Vector3();
596
597 MathHelper.PlaneSpace1(cp.NormalWorldOnB, ref frictionTangential0a, ref frictionTangential1b);
598 {
599 SolverConstraint solverConstraint = new SolverConstraint();
600 _tmpSolverFrictionConstraintPool.Add(solverConstraint);
601 solverConstraint.ContactNormal = frictionTangential0a;
602
603 solverConstraint.SolverBodyIdA = solverBodyIdA;
604 solverConstraint.SolverBodyIdB = solverBodyIdB;
605 solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction;
606 solverConstraint.FrictionIndex = frictionIndex;
607
608 solverConstraint.Friction = cp.CombinedFriction;
609
610 solverConstraint.AppliedImpulse = 0;
611 solverConstraint.AppliedVelocityImpulse = 0;
612
613 float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal);
614 float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal);
615 float denom = relaxation / (denom0 + denom1);
616 solverConstraint.JacDiagABInv = denom;
617
618 {
619 Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal);
620 solverConstraint.RelPosACrossNormal = ftorqueAxis0;
621 solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis0, rb0.InvInertiaTensorWorld);
622 }
623 {
624 Vector3 ftorqueAxis0 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal);
625 solverConstraint.RelPosBCrossNormal = ftorqueAxis0;
626 solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis0, rb1.InvInertiaTensorWorld);
627 }
628 }
629
630
631 {
632
633 SolverConstraint solverConstraint = new SolverConstraint();
634 _tmpSolverFrictionConstraintPool.Add(solverConstraint);
635 solverConstraint.ContactNormal = frictionTangential1b;
636
637 solverConstraint.SolverBodyIdA = solverBodyIdA;
638 solverConstraint.SolverBodyIdB = solverBodyIdB;
639 solverConstraint.ConstraintType = SolverConstraint.SolverConstraintType.Friction;
640 solverConstraint.FrictionIndex = frictionIndex;
641
642 solverConstraint.Friction = cp.CombinedFriction;
643
644 solverConstraint.AppliedImpulse = 0;
645 solverConstraint.AppliedVelocityImpulse = 0;
646
647 float denom0 = rb0.ComputeImpulseDenominator(pos1, solverConstraint.ContactNormal);
648 float denom1 = rb1.ComputeImpulseDenominator(pos2, solverConstraint.ContactNormal);
649 float denom = relaxation / (denom0 + denom1);
650 solverConstraint.JacDiagABInv = denom;
651 {
652 Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos1, solverConstraint.ContactNormal);
653 solverConstraint.RelPosACrossNormal = ftorqueAxis1;
654 solverConstraint.AngularComponentA = Vector3.TransformNormal(ftorqueAxis1, rb0.InvInertiaTensorWorld);
655 }
656 {
657 Vector3 ftorqueAxis1 = Vector3.Cross(rel_pos2, solverConstraint.ContactNormal);
658 solverConstraint.RelPosBCrossNormal = ftorqueAxis1;
659 solverConstraint.AngularComponentB = Vector3.TransformNormal(ftorqueAxis1, rb1.InvInertiaTensorWorld);
660 }
661 }
662 }
663 }
664 }
665
666 ContactSolverInfo info = infoGlobal;
667 {
668 for (int j = 0; j < constraints.Count; j++)
669 {
670 TypedConstraint constraint = constraints[j];
671 constraint.BuildJacobian();
672 }
673 }
674
675 int numConstraintPool = _tmpSolverConstraintPool.Count;
676 int numFrictionPool = _tmpSolverFrictionConstraintPool.Count;
677
678 //todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
679 List<int> gOrderTmpConstraintPool = new List<int>(numConstraintPool);
680 List<int> gOrderFrictionConstraintPool = new List<int>(numFrictionPool);
681 {
682 for (int i = 0; i < numConstraintPool; i++)
683 {
684 gOrderTmpConstraintPool.Add(i);
685 }
686 for (int i = 0; i < numFrictionPool; i++)
687 {
688 gOrderFrictionConstraintPool.Add(i);
689 }
690 }
691
692 //should traverse the contacts random order...
693 int iteration;
694 {
695 for (iteration = 0; iteration < info.IterationsCount; iteration++)
696 {
697
698 int j;
699 if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None)
700 {
701 if ((iteration & 7) == 0)
702 {
703 for (j = 0; j < numConstraintPool; ++j)
704 {
705 int tmp = gOrderTmpConstraintPool[j];
706 int swapi = RandInt2(j + 1);
707 gOrderTmpConstraintPool[j] = gOrderTmpConstraintPool[swapi];
708 gOrderTmpConstraintPool[swapi] = tmp;
709 }
710
711 for (j = 0; j < numFrictionPool; ++j)
712 {
713 int tmp = gOrderFrictionConstraintPool[j];
714 int swapi = RandInt2(j + 1);
715 gOrderFrictionConstraintPool[j] = gOrderFrictionConstraintPool[swapi];
716 gOrderFrictionConstraintPool[swapi] = tmp;
717 }
718 }
719 }
720
721 for (j = 0; j < constraints.Count; j++)
722 {
723 TypedConstraint constraint = constraints[j];
724 //todo: use solver bodies, so we don't need to copy from/to btRigidBody
725
726 if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0))
727 {
728 _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].WriteBackVelocity();
729 }
730 if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0))
731 {
732 _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].WriteBackVelocity();
733 }
734
735 constraint.SolveConstraint(info.TimeStep);
736
737 if ((constraint.RigidBodyA.IslandTag >= 0) && (constraint.RigidBodyA.CompanionID >= 0))
738 {
739 _tmpSolverBodyPool[constraint.RigidBodyA.CompanionID].ReadVelocity();
740 }
741 if ((constraint.RigidBodyB.IslandTag >= 0) && (constraint.RigidBodyB.CompanionID >= 0))
742 {
743 _tmpSolverBodyPool[constraint.RigidBodyB.CompanionID].ReadVelocity();
744 }
745
746 }
747
748 {
749 int numPoolConstraints = _tmpSolverConstraintPool.Count;
750 for (j = 0; j < numPoolConstraints; j++)
751 {
752 SolverConstraint solveManifold = _tmpSolverConstraintPool[gOrderTmpConstraintPool[j]];
753 ResolveSingleCollisionCombinedCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA],
754 _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info);
755 }
756 }
757
758 {
759 int numFrictionPoolConstraints = _tmpSolverFrictionConstraintPool.Count;
760 for (j = 0; j < numFrictionPoolConstraints; j++)
761 {
762 SolverConstraint solveManifold = _tmpSolverFrictionConstraintPool[gOrderFrictionConstraintPool[j]];
763 float appliedNormalImpulse = _tmpSolverConstraintPool[solveManifold.FrictionIndex].AppliedImpulse;
764
765 ResolveSingleFrictionCacheFriendly(_tmpSolverBodyPool[solveManifold.SolverBodyIdA],
766 _tmpSolverBodyPool[solveManifold.SolverBodyIdB], solveManifold, info, appliedNormalImpulse);
767 }
768 }
769 }
770 }
771
772 for (int i = 0; i < _tmpSolverBodyPool.Count; i++)
773 {
774 _tmpSolverBodyPool[i].WriteBackVelocity();
775 }
776
777 _tmpSolverBodyPool.Clear();
778 _tmpSolverConstraintPool.Clear();
779 _tmpSolverFrictionConstraintPool.Clear();
780
781 return 0;
782 }
783
784 public virtual float SolveGroup(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, List<TypedConstraint> constraints, ContactSolverInfo infoGlobal, IDebugDraw debugDrawer)
785 {
786 if ((_solverMode & SolverMode.CacheFriendly) != SolverMode.None)
787 {
788 return SolveGroupCacheFriendly(bodies, manifolds, numManifolds, constraints, infoGlobal, debugDrawer);
789 }
790
791 ContactSolverInfo info = infoGlobal;
792 int totalPoints = 0;
793
794 int numiter = infoGlobal.IterationsCount;
795
796 for (int j = 0; j < manifolds.Count; j++)
797 {
798 PersistentManifold manifold = manifolds[j];
799 PrepareConstraints(manifold, info);
800
801 for (int p = 0; p < manifolds[j].ContactsCount; p++)
802 {
803 _order[totalPoints].ManifoldIndex = j;
804 _order[totalPoints].PointIndex = p;
805 totalPoints++;
806 }
807 }
808
809 for (int j = 0; j < constraints.Count; j++)
810 {
811 constraints[j].BuildJacobian();
812 }
813
814 //should traverse the contacts random order...
815 int iteration;
816
817 for (iteration = 0; iteration < numiter; iteration++)
818 {
819 int j;
820 if ((_solverMode & SolverMode.RandomizeOrder) != SolverMode.None)
821 {
822 if ((iteration & 7) == 0)
823 {
824 for (j = 0; j < totalPoints; ++j)
825 {
826 OrderIndex tmp = _order[j];
827 int swapi = RandInt2(j + 1);
828 _order[j] = _order[swapi];
829 _order[swapi] = tmp;
830 }
831 }
832 }
833
834 for (j = 0; j < constraints.Count; j++)
835 {
836 constraints[j].SolveConstraint(info.TimeStep);
837 }
838
839 for (j = 0; j < totalPoints; j++)
840 {
841 PersistentManifold manifold = manifolds[_order[j].ManifoldIndex];
842 Solve((RigidBody)manifold.BodyA, (RigidBody)manifold.BodyB,
843 manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer);
844 }
845
846 for (j = 0; j < totalPoints; j++)
847 {
848 PersistentManifold manifold = manifolds[_order[j].ManifoldIndex];
849 SolveFriction((RigidBody)manifold.BodyA,
850 (RigidBody)manifold.BodyB, manifold.GetContactPoint(_order[j].PointIndex), info, iteration, debugDrawer);
851 }
852 }
853
854 return 0;
855 }
856
857 private void InitSolverBody(out SolverBody solverBody, RigidBody rigidBody)
858 {
859 solverBody = new SolverBody();
860 solverBody.AngularVelocity = rigidBody.AngularVelocity;
861 solverBody.CenterOfMassPosition = rigidBody.CenterOfMassPosition;
862 solverBody.Friction = rigidBody.Friction;
863 solverBody.InvMass = rigidBody.InverseMass;
864 solverBody.LinearVelocity = rigidBody.LinearVelocity;
865 solverBody.OriginalBody = rigidBody;
866 solverBody.AngularFactor = rigidBody.AngularFactor;
867 }
868
869 private long Rand2()
870 {
871 _seed2 = (1664525L * _seed2 + 1013904223L) & 0xffffffff;
872 return _seed2;
873 }
874
875 private int RandInt2(int n)
876 {
877 // seems good; xor-fold and modulus
878 long un = n;
879 long r = Rand2();
880
881 // note: probably more aggressive than it needs to be -- might be
882 // able to get away without one or two of the innermost branches.
883 if (un <= 0x00010000L)
884 {
885 r ^= (r >> 16);
886 if (un <= 0x00000100L)
887 {
888 r ^= (r >> 8);
889 if (un <= 0x00000010L)
890 {
891 r ^= (r >> 4);
892 if (un <= 0x00000004L)
893 {
894 r ^= (r >> 2);
895 if (un <= 0x00000002L)
896 {
897 r ^= (r >> 1);
898 }
899 }
900 }
901 }
902 }
903 return (int)(r % un);
904 }
905
906 protected struct OrderIndex
907 {
908 private int _manifoldIndex;
909 private int _pointIndex;
910
911 public int ManifoldIndex { get { return _manifoldIndex; } set { _manifoldIndex = value; } }
912 public int PointIndex { get { return _pointIndex; } set { _pointIndex = value; } }
913 }
914 }
915}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs
new file mode 100644
index 0000000..6ced783
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/Solve2LinearConstraint.cs
@@ -0,0 +1,188 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX.Dynamics
28{
29 /// <summary>
30 /// constraint class used for lateral tyre friction
31 /// </summary>
32 public class Solve2LinearConstraint
33 {
34 private float _tau;
35 private float _damping;
36
37 public Solve2LinearConstraint(float tau, float damping)
38 {
39 _tau = tau;
40 _damping = damping;
41 }
42
43 // solve unilateral constraint (equality, direct method)
44 public void ResolveUnilateralPairConstraint(
45 RigidBody body1, RigidBody body2,
46 Matrix world2A,
47 Matrix world2B,
48 Vector3 invInertiaADiag,
49 float invMassA,
50 Vector3 linvelA, Vector3 angvelA,
51 Vector3 rel_posA1,
52 Vector3 invInertiaBDiag,
53 float invMassB,
54 Vector3 linvelB, Vector3 angvelB,
55 Vector3 rel_posA2,
56 float depthA, Vector3 normalA,
57 Vector3 rel_posB1, Vector3 rel_posB2,
58 float depthB, Vector3 normalB,
59 out float imp0, out float imp1)
60 {
61 imp0 = 0;
62 imp1 = 0;
63
64 float len = Math.Abs(normalA.Length()) - 1f;
65 if (Math.Abs(len) >= float.Epsilon)
66 return;
67
68 BulletDebug.Assert(len < float.Epsilon);
69
70 //this jacobian entry could be re-used for all iterations
71 JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
72 invInertiaBDiag, invMassB);
73 JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
74 invInertiaBDiag, invMassB);
75
76 float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1));
77 float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1));
78
79 // btScalar penetrationImpulse = (depth*contactTau*timeCorrection) * massTerm;//jacDiagABInv
80 float massTerm = 1f / (invMassA + invMassB);
81
82 // calculate rhs (or error) terms
83 float dv0 = depthA * _tau * massTerm - vel0 * _damping;
84 float dv1 = depthB * _tau * massTerm - vel1 * _damping;
85
86 float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);
87 float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag);
88
89 imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet;
90 imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet;
91 }
92
93 // solving 2x2 lcp problem (inequality, direct solution )
94 public void ResolveBilateralPairConstraint(
95 RigidBody body1, RigidBody body2,
96 Matrix world2A, Matrix world2B,
97 Vector3 invInertiaADiag,
98 float invMassA,
99 Vector3 linvelA, Vector3 angvelA,
100 Vector3 rel_posA1,
101 Vector3 invInertiaBDiag,
102 float invMassB,
103 Vector3 linvelB, Vector3 angvelB,
104 Vector3 rel_posA2,
105 float depthA, Vector3 normalA,
106 Vector3 rel_posB1, Vector3 rel_posB2,
107 float depthB, Vector3 normalB,
108 out float imp0, out float imp1)
109 {
110 imp0 = 0f;
111 imp1 = 0f;
112
113 float len = Math.Abs(normalA.Length()) - 1f;
114 if (Math.Abs(len) >= float.Epsilon)
115 return;
116
117 BulletDebug.Assert(len < float.Epsilon);
118
119 JacobianEntry jacA = new JacobianEntry(world2A, world2B, rel_posA1, rel_posA2, normalA, invInertiaADiag, invMassA,
120 invInertiaBDiag, invMassB);
121 JacobianEntry jacB = new JacobianEntry(world2A, world2B, rel_posB1, rel_posB2, normalB, invInertiaADiag, invMassA,
122 invInertiaBDiag, invMassB);
123
124 float vel0 = Vector3.Dot(normalA, body1.GetVelocityInLocalPoint(rel_posA1) - body2.GetVelocityInLocalPoint(rel_posA1));
125 float vel1 = Vector3.Dot(normalB, body1.GetVelocityInLocalPoint(rel_posB1) - body2.GetVelocityInLocalPoint(rel_posB1));
126
127 // calculate rhs (or error) terms
128 float dv0 = depthA * _tau - vel0 * _damping;
129 float dv1 = depthB * _tau - vel1 * _damping;
130
131 float nonDiag = jacA.GetNonDiagonal(jacB, invMassA, invMassB);
132 float invDet = 1.0f / (jacA.Diagonal * jacB.Diagonal - nonDiag * nonDiag);
133
134 imp0 = dv0 * jacA.Diagonal * invDet + dv1 * -nonDiag * invDet;
135 imp1 = dv1 * jacB.Diagonal * invDet + dv0 * -nonDiag * invDet;
136
137 if (imp0 > 0.0f)
138 {
139 if (imp1 <= 0.0f)
140 {
141 imp1 = 0f;
142
143 // now imp0>0 imp1<0
144 imp0 = dv0 / jacA.Diagonal;
145 if (imp0 < 0.0f)
146 imp0 = 0f;
147 }
148 }
149 else
150 {
151 imp0 = 0f;
152
153 imp1 = dv1 / jacB.Diagonal;
154 if (imp1 <= 0.0f)
155 {
156 imp1 = 0f;
157 // now imp0>0 imp1<0
158 imp0 = dv0 / jacA.Diagonal;
159 if (imp0 > 0.0f)
160 {
161 }
162 else
163 {
164 imp0 = 0f;
165 }
166 }
167 }
168 }
169
170 //public void ResolveAngularConstraint(
171 // Matrix invInertiaAWS,
172 // float invMassA,
173 // Vector3 linvelA, Vector3 angvelA,
174 // Vector3 rel_posA1,
175 // Matrix invInertiaBWS,
176 // float invMassB,
177 // Vector3 linvelB, Vector3 angvelB,
178 // Vector3 rel_posA2,
179 // float depthA, Vector3 normalA,
180 // Vector3 rel_posB1, Vector3 rel_posB2,
181 // float depthB, Vector3 normalB,
182 // out float imp0, out float imp1)
183 //{
184 // imp0 = 0;
185 // imp1 = 0;
186 //}
187 }
188}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs
new file mode 100644
index 0000000..09b73cc
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverBody.cs
@@ -0,0 +1,78 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26using System.Runtime.InteropServices;
27
28namespace XnaDevRu.BulletX.Dynamics
29{
30 public class SolverBody
31 {
32 private Vector3 _centerOfMassPosition = new Vector3();
33 private Vector3 _linearVelocity = new Vector3();
34 private Vector3 _angularVelocity = new Vector3();
35 private RigidBody _originalBody = null;
36 private float _invMass;
37 private float _friction;
38 private float _angularFactor;
39
40 public Vector3 CenterOfMassPosition { get { return _centerOfMassPosition; } set { _centerOfMassPosition = value; } }
41 public Vector3 LinearVelocity { get { return _linearVelocity; } set { _linearVelocity = value; } }
42 public Vector3 AngularVelocity { get { return _angularVelocity; } set { _angularVelocity = value; } }
43 public RigidBody OriginalBody { get { return _originalBody; } set { _originalBody = value; } }
44 public float InvMass { get { return _invMass; } set { _invMass = value; } }
45 public float Friction { get { return _friction; } set { _friction = value; } }
46 public float AngularFactor { get { return _angularFactor; } set { _angularFactor = value; } }
47
48 public void GetVelocityInLocalPoint(Vector3 relPos, out Vector3 velocity)
49 {
50 velocity = _linearVelocity + Vector3.Cross(_angularVelocity, relPos);
51 }
52
53 public void WriteBackVelocity()
54 {
55 if (_invMass != 0)
56 {
57 _originalBody.LinearVelocity = _linearVelocity;
58 _originalBody.AngularVelocity = _angularVelocity;
59 }
60 }
61
62 public void ReadVelocity()
63 {
64 if (_invMass != 0)
65 {
66 _linearVelocity = _originalBody.LinearVelocity;
67 _angularVelocity = _originalBody.AngularVelocity;
68 }
69 }
70
71 //Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
72 internal void ApplyImpulse(Vector3 linearComponent, Vector3 angularComponent, float impulseMagnitude)
73 {
74 _linearVelocity += linearComponent * impulseMagnitude;
75 _angularVelocity += angularComponent * impulseMagnitude * _angularFactor;
76 }
77 }
78}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs
new file mode 100644
index 0000000..3e3a64a
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/SolverConstraint.cs
@@ -0,0 +1,77 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26using System.Runtime.InteropServices;
27
28namespace XnaDevRu.BulletX.Dynamics
29{
30 //1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and friction constraints.
31 public class SolverConstraint
32 {
33 private Vector3 _relpos1CrossNormal = new Vector3();
34 private Vector3 _relpos2CrossNormal = new Vector3();
35 private Vector3 _contactNormal = new Vector3();
36 private Vector3 _angularComponentA = new Vector3();
37 private Vector3 _angularComponentB = new Vector3();
38
39 private float _appliedVelocityImpulse;
40 private int _solverBodyIdA;
41 int _solverBodyIdB;
42 private float _friction;
43 private float _restitution;
44 private float _jacDiagABInv;
45 private float _penetration;
46 private float _appliedImpulse;
47
48 private SolverConstraintType _constraintType = SolverConstraintType.Contact;
49 private int _frictionIndex;
50 private int[] _unusedPadding = new int[2];
51
52 public Vector3 RelPosACrossNormal { get { return _relpos1CrossNormal; } set { _relpos1CrossNormal = value; } }
53 public Vector3 RelPosBCrossNormal { get { return _relpos2CrossNormal; } set { _relpos2CrossNormal = value; } }
54 public Vector3 ContactNormal { get { return _contactNormal; } set { _contactNormal = value; } }
55 public Vector3 AngularComponentA { get { return _angularComponentA; } set { _angularComponentA = value; } }
56 public Vector3 AngularComponentB { get { return _angularComponentB; } set { _angularComponentB = value; } }
57
58 public float AppliedVelocityImpulse { get { return _appliedVelocityImpulse; } set { _appliedVelocityImpulse = value; } }
59 public int SolverBodyIdA { get { return _solverBodyIdA; } set { _solverBodyIdA = value; } }
60 public int SolverBodyIdB { get { return _solverBodyIdB; } set { _solverBodyIdB = value; } }
61 public float Friction { get { return _friction; } set { _friction = value; } }
62 public float Restitution { get { return _restitution; } set { _restitution = value; } }
63 public float JacDiagABInv { get { return _jacDiagABInv; } set { _jacDiagABInv = value; } }
64 public float Penetration { get { return _penetration; } set { _penetration = value; } }
65 public float AppliedImpulse { get { return _appliedImpulse; } set { _appliedImpulse = value; } }
66
67 public SolverConstraintType ConstraintType { get { return _constraintType; } set { _constraintType = value; } }
68 public int FrictionIndex { get { return _frictionIndex; } set { _frictionIndex = value; } }
69 public int[] UnusedPadding { get { return _unusedPadding; } set { _unusedPadding = value; } }
70
71 public enum SolverConstraintType
72 {
73 Contact = 0,
74 Friction,
75 }
76 }
77}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs
new file mode 100644
index 0000000..abd10f1
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/ConstraintSolver/TypedConstraint.cs
@@ -0,0 +1,88 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX.Dynamics
28{
29 public abstract class TypedConstraint
30 {
31 private static RigidBody _fixed = new RigidBody(0, null, null, new Vector3(), 0, 0, 0.5f, 0);
32 private int _userConstraintType;
33 private int _userConstraintId;
34
35 private RigidBody _rbA;
36 private RigidBody _rbB;
37 private float _appliedImpulse;
38
39 public TypedConstraint()
40 : this(_fixed, _fixed) { }
41
42 public TypedConstraint(RigidBody rbA)
43 : this(rbA, _fixed) { }
44
45 public TypedConstraint(RigidBody rbA, RigidBody rbB)
46 {
47 _userConstraintType = -1;
48 _userConstraintId = -1;
49 _rbA = rbA;
50 _rbB = rbB;
51 _appliedImpulse = 0;
52
53 _fixed.SetMassProps(0, new Vector3());
54 }
55
56 public virtual RigidBody RigidBodyA { get { return _rbA; } protected set { _rbA = value; } }
57 public virtual RigidBody RigidBodyB { get { return _rbB; } protected set { _rbB = value; } }
58
59 public float AppliedImpulse { get { return _appliedImpulse; } protected set { _appliedImpulse = value; } }
60 public int UserConstraintId { get { return _userConstraintId; } set { _userConstraintId = value; } }
61 public int UserConstraintType { get { return _userConstraintType; } set { _userConstraintType = value; } }
62
63 public abstract void BuildJacobian();
64 public abstract void SolveConstraint(float timeStep);
65
66 public static int SortConstraintOnIslandPredicate(TypedConstraint left, TypedConstraint right)
67 {
68 int rightIslandID, leftIslandID;
69 rightIslandID = GetConstraintIslandId(right);
70 leftIslandID = GetConstraintIslandId(left);
71 if (leftIslandID < rightIslandID)
72 return -1;
73 else
74 return 1;
75 return 0;
76 }
77
78 internal static int GetConstraintIslandId(TypedConstraint lhs)
79 {
80 int islandId;
81
82 CollisionObject colObjA = lhs.RigidBodyA;
83 CollisionObject colObjB = lhs.RigidBodyB;
84 islandId = colObjA.IslandTag >= 0 ? colObjA.IslandTag : colObjB.IslandTag;
85 return islandId;
86 }
87 }
88}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs
new file mode 100644
index 0000000..acceb98
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DiscreteDynamicsWorld.cs
@@ -0,0 +1,790 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX.Dynamics
28{
29 /// <summary>
30 /// DiscreteDynamicsWorld provides discrete rigid body simulation
31 /// those classes replace the obsolete CcdPhysicsEnvironment/CcdPhysicsController
32 /// </summary>
33 public class DiscreteDynamicsWorld : DynamicsWorld
34 {
35 private static bool _reportMe = true;
36
37 private IConstraintSolver _constraintSolver;
38 private SimulationIslandManager _islandManager;
39 private List<TypedConstraint> _constraints = new List<TypedConstraint>();
40 private IDebugDraw _debugDrawer;
41 private ContactSolverInfo _solverInfo = new ContactSolverInfo();
42 private Vector3 _gravity;
43 //for variable timesteps
44 private float _localTime;
45 //for variable timesteps
46 private bool _ownsIslandManager;
47 private bool _ownsConstraintSolver;
48 private List<RaycastVehicle> _vehicles = new List<RaycastVehicle>();
49 private int _profileTimings;
50
51 public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache)
52 : this(dispatcher, pairCache, null) { }
53
54 //this btDiscreteDynamicsWorld constructor gets created objects from the user, and will not delete those
55 public DiscreteDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver)
56 : base(dispatcher, pairCache)
57 {
58 _constraintSolver = constraintSolver != null ? constraintSolver : new SequentialImpulseConstraintSolver();
59 _debugDrawer = null;
60 _gravity = new Vector3(0, -10, 0);
61 _localTime = 1f / 60f;
62 _profileTimings = 0;
63 _islandManager = new SimulationIslandManager();
64 _ownsIslandManager = true;
65 _ownsConstraintSolver = constraintSolver == null;
66 }
67
68 public ContactSolverInfo SolverInfo { get { return _solverInfo; } }
69 public SimulationIslandManager SimulationIslandManager { get { return _islandManager; } }
70 public CollisionWorld CollisionWorld { get { return this; } }
71
72 public override IDebugDraw DebugDrawer
73 {
74 get
75 {
76 return _debugDrawer;
77 }
78 set
79 {
80 _debugDrawer = value;
81 }
82 }
83
84 public override Vector3 Gravity
85 {
86 set
87 {
88 _gravity = value;
89 for (int i = 0; i < CollisionObjects.Count; i++)
90 {
91 CollisionObject colObj = CollisionObjects[i];
92 RigidBody body = RigidBody.Upcast(colObj);
93 if (body != null)
94 {
95 body.Gravity = value;
96 }
97 }
98 }
99 }
100
101 public override IConstraintSolver ConstraintSolver
102 {
103 set
104 {
105 _ownsConstraintSolver = false;
106 _constraintSolver = value;
107 }
108 }
109
110 public override int ConstraintsCount
111 {
112 get
113 {
114 return _constraints.Count;
115 }
116 }
117
118 //if maxSubSteps > 0, it will interpolate motion between fixedTimeStep's
119 public override void StepSimulation(float timeStep, int maxSubSteps, float fixedTimeStep)
120 {
121 int numSimulationSubSteps = 0;
122
123 if (maxSubSteps != 0)
124 {
125 //fixed timestep with interpolation
126 _localTime += timeStep;
127 if (_localTime >= fixedTimeStep)
128 {
129 numSimulationSubSteps = (int)(_localTime / fixedTimeStep);
130 _localTime -= numSimulationSubSteps * fixedTimeStep;
131 }
132 }
133 else
134 {
135 //variable timestep
136 fixedTimeStep = timeStep;
137 _localTime = timeStep;
138 if (Math.Abs(timeStep) < float.Epsilon)
139 {
140 numSimulationSubSteps = 0;
141 maxSubSteps = 0;
142 }
143 else
144 {
145 numSimulationSubSteps = 1;
146 maxSubSteps = 1;
147 }
148 }
149
150 //process some debugging flags
151 if (DebugDrawer != null)
152 {
153 RigidBody.DisableDeactivation = (DebugDrawer.DebugMode & DebugDrawModes.NoDeactivation) != 0;
154 }
155 if (numSimulationSubSteps != 0)
156 {
157
158 SaveKinematicState(fixedTimeStep);
159
160 //clamp the number of substeps, to prevent simulation grinding spiralling down to a halt
161 int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps) ? maxSubSteps : numSimulationSubSteps;
162
163 for (int i = 0; i < clampedSimulationSteps; i++)
164 {
165 InternalSingleStepSimulation(fixedTimeStep);
166 SynchronizeMotionStates();
167 }
168
169 }
170
171 SynchronizeMotionStates();
172
173 //return numSimulationSubSteps;
174 }
175
176 public void StepSimulation(float timeStep, int maxSubSteps)
177 {
178 StepSimulation(timeStep, maxSubSteps, 1f / 60f);
179 }
180
181 public override void UpdateAabbs()
182 {
183 Vector3 colorvec = new Vector3(1, 0, 0);
184 for (int i = 0; i < CollisionObjects.Count; i++)
185 {
186 CollisionObject colObj = CollisionObjects[i];
187 RigidBody body = RigidBody.Upcast(colObj);
188
189 if (body != null)
190 {
191 // if (body->IsActive() && (!body->IsStatic()))
192 {
193 Vector3 minAabb, maxAabb;
194 colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb);
195 OverlappingPairCache bp = BroadphasePairCache;
196
197 //moving objects should be moderately sized, probably something wrong if not
198 if (colObj.IsStaticObject || ((maxAabb - minAabb).LengthSquared() < 1e12f))
199 {
200 bp.SetAabb(body.Broadphase, minAabb, maxAabb);
201 }
202 else
203 {
204 //something went wrong, investigate
205 //this assert is unwanted in 3D modelers (danger of loosing work)
206 BulletDebug.Assert(false);
207 body.ActivationState = ActivationState.DisableSimulation;
208
209 if (_reportMe)
210 {
211 _reportMe = false;
212 Console.WriteLine("Overflow in AABB, object removed from simulation \n");
213 Console.WriteLine("If you can reproduce this, please email bugs@continuousphysics.com\n");
214 Console.WriteLine("Please include above information, your Platform, version of OS.\n");
215 Console.WriteLine("Thanks.\n");
216 }
217 }
218 if (_debugDrawer != null && (_debugDrawer.DebugMode & DebugDrawModes.DrawAabb) != 0)
219 DrawAabb(_debugDrawer, minAabb, maxAabb, colorvec);
220 }
221 }
222 }
223 }
224
225 public override void AddConstraint(TypedConstraint constraint)
226 {
227 _constraints.Add(constraint);
228 }
229
230 public override void RemoveConstraint(TypedConstraint constraint)
231 {
232 _constraints.Remove(constraint);
233 }
234
235 public void AddVehicle(RaycastVehicle vehicle)
236 {
237 _vehicles.Add(vehicle);
238 }
239
240 public void RemoveVehicle(RaycastVehicle vehicle)
241 {
242 _vehicles.Remove(vehicle);
243 }
244
245 public override void AddRigidBody(RigidBody body)
246 {
247 if (!body.IsStaticOrKinematicObject)
248 {
249 body.Gravity = _gravity;
250 }
251
252 if (body.CollisionShape != null)
253 {
254 bool isDynamic = !(body.IsStaticObject || body.IsKinematicObject);
255 BroadphaseProxy.CollisionFilterGroups collisionFilterGroup = isDynamic ? BroadphaseProxy.CollisionFilterGroups.Default : BroadphaseProxy.CollisionFilterGroups.Static;
256 BroadphaseProxy.CollisionFilterGroups collisionFilterMask = isDynamic ? BroadphaseProxy.CollisionFilterGroups.All : (BroadphaseProxy.CollisionFilterGroups.All ^ BroadphaseProxy.CollisionFilterGroups.Static);
257
258 AddCollisionObject(body, collisionFilterGroup, collisionFilterMask);
259 }
260 }
261
262 public override void RemoveRigidBody(RigidBody body)
263 {
264 RemoveCollisionObject(body);
265 }
266
267 public void DebugDrawObject(Matrix worldTransform, CollisionShape shape, Vector3 color)
268 {
269 if (shape.ShapeType == BroadphaseNativeTypes.Compound)
270 {
271 CompoundShape compoundShape = shape as CompoundShape;
272 for (int i = compoundShape.ChildShapeCount - 1; i >= 0; i--)
273 {
274 Matrix childTrans = compoundShape.GetChildTransform(i);
275 CollisionShape colShape = compoundShape.GetChildShape(i);
276 DebugDrawObject(worldTransform * childTrans, colShape, color);
277 }
278
279 }
280 else
281 {
282 switch (shape.ShapeType)
283 {
284
285 case BroadphaseNativeTypes.Sphere:
286 {
287 SphereShape sphereShape = shape as SphereShape;
288 float radius = sphereShape.Margin;//radius doesn't include the margin, so draw with margin
289 Vector3 start = worldTransform.Translation;
290 DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(radius, 0, 0), worldTransform), color);
291 DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, radius, 0), worldTransform), color);
292 DebugDrawer.DrawLine(start, start + Vector3.TransformNormal(new Vector3(0, 0, radius), worldTransform), color);
293 //drawSphere
294 break;
295 }
296 case BroadphaseNativeTypes.MultiSphere:
297 case BroadphaseNativeTypes.Cone:
298 {
299 ConeShape coneShape = shape as ConeShape;
300 float radius = coneShape.Radius;//+coneShape->getMargin();
301 float height = coneShape.Height;//+coneShape->getMargin();
302 Vector3 start = worldTransform.Translation;
303 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);
304 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);
305 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);
306 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);
307 break;
308 }
309 case BroadphaseNativeTypes.Cylinder:
310 {
311 CylinderShape cylinder = shape as CylinderShape;
312 int upAxis = cylinder.UpAxis;
313 float radius = cylinder.Radius;
314 float halfHeight = MathHelper.GetElement(cylinder.HalfExtents, upAxis);
315 Vector3 start = worldTransform.Translation;
316 Vector3 offsetHeight = new Vector3();
317 MathHelper.SetElement(ref offsetHeight, upAxis, halfHeight);
318 Vector3 offsetRadius = new Vector3();
319 MathHelper.SetElement(ref offsetRadius, (upAxis + 1) % 3, radius);
320 DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight + offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight + offsetRadius, worldTransform), color);
321 DebugDrawer.DrawLine(start + Vector3.TransformNormal(offsetHeight - offsetRadius, worldTransform), start + Vector3.TransformNormal(-offsetHeight - offsetRadius, worldTransform), color);
322 break;
323 }
324 default:
325 {
326 if (shape.ShapeType == BroadphaseNativeTypes.TriangleMesh)
327 {
328 TriangleMeshShape concaveMesh = shape as TriangleMeshShape;
329 //btVector3 aabbMax(1e30f,1e30f,1e30f);
330 //btVector3 aabbMax(100,100,100);//1e30f,1e30f,1e30f);
331
332 //todo pass camera, for some culling
333 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
334 Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
335
336 DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color);
337 concaveMesh.ProcessAllTriangles(drawCallback, aabbMin, aabbMax);
338 }
339
340 if (shape.ShapeType == BroadphaseNativeTypes.ConvexTriangleMesh)
341 {
342 ConvexTriangleMeshShape convexMesh = shape as ConvexTriangleMeshShape;
343 //todo: pass camera for some culling
344 Vector3 aabbMax = new Vector3(1e30f, 1e30f, 1e30f);
345 Vector3 aabbMin = new Vector3(-1e30f, -1e30f, -1e30f);
346 //DebugDrawcallback drawCallback;
347 DebugDrawCallback drawCallback = new DebugDrawCallback(DebugDrawer, worldTransform, color);
348 convexMesh.getStridingMesh().InternalProcessAllTriangles(drawCallback, aabbMin, aabbMax);
349 }
350
351 // for polyhedral shapes
352 if (shape.IsPolyhedral)
353 {
354 PolyhedralConvexShape polyshape = shape as PolyhedralConvexShape;
355
356 for (int i = 0; i < polyshape.EdgeCount; i++)
357 {
358 Vector3 a, b;
359 polyshape.GetEdge(i, out a, out b);
360 a = Vector3.TransformNormal(a, worldTransform);
361 b = Vector3.TransformNormal(b, worldTransform);
362 DebugDrawer.DrawLine(a, b, color);
363 }
364 }
365 break;
366 }
367 }
368 }
369 }
370
371 public override TypedConstraint GetConstraint(int index)
372 {
373 return _constraints[index];
374 }
375
376 public static void DrawAabb(IDebugDraw debugDrawer, Vector3 from, Vector3 to, Vector3 color)
377 {
378 Vector3 halfExtents = (to - from) * 0.5f;
379 Vector3 center = (to + from) * 0.5f;
380
381 Vector3 edgecoord = new Vector3(1f, 1f, 1f), pa, pb;
382 for (int i = 0; i < 4; i++)
383 {
384 for (int j = 0; j < 3; j++)
385 {
386 pa = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y,
387 edgecoord.Z * halfExtents.Z);
388 pa += center;
389
390 int othercoord = j % 3;
391 MathHelper.SetElement(ref edgecoord, othercoord, MathHelper.GetElement(edgecoord, othercoord) * -1f);
392 pb = new Vector3(edgecoord.X * halfExtents.X, edgecoord.Y * halfExtents.Y,
393 edgecoord.Z * halfExtents.Z);
394 pb += center;
395
396 debugDrawer.DrawLine(pa, pb, color);
397 }
398 edgecoord = new Vector3(-1f, -1f, -1f);
399 if (i < 3)
400 MathHelper.SetElement(ref edgecoord, i, MathHelper.GetElement(edgecoord, i) * -1f);
401 }
402 }
403
404 protected void PredictUnconstraintMotion(float timeStep)
405 {
406 for (int i = 0; i < CollisionObjects.Count; i++)
407 {
408 CollisionObject colObj = CollisionObjects[i];
409 RigidBody body = RigidBody.Upcast(colObj);
410 if (body != null)
411 {
412 if (!body.IsStaticOrKinematicObject)
413 {
414 if (body.IsActive)
415 {
416 body.ApplyForces(timeStep);
417 body.IntegrateVelocities(timeStep);
418 Matrix temp = body.InterpolationWorldTransform;
419 body.PredictIntegratedTransform(timeStep, ref temp);
420 body.InterpolationWorldTransform = temp;
421 }
422 }
423 }
424 }
425 }
426
427 protected void IntegrateTransforms(float timeStep)
428 {
429 Matrix predictedTrans = new Matrix();
430 for (int i = 0; i < CollisionObjects.Count; i++)
431 {
432 CollisionObject colObj = CollisionObjects[i];
433 RigidBody body = RigidBody.Upcast(colObj);
434 if (body != null)
435 {
436 if (body.IsActive && (!body.IsStaticOrKinematicObject))
437 {
438 body.PredictIntegratedTransform(timeStep, ref predictedTrans);
439 body.ProceedToTransform(predictedTrans);
440 }
441 }
442 }
443 }
444
445 protected void CalculateSimulationIslands()
446 {
447 SimulationIslandManager.UpdateActivationState(this, Dispatcher);
448
449 for (int i = 0; i < _constraints.Count; i++)
450 {
451 TypedConstraint constraint = _constraints[i];
452
453 RigidBody colObj0 = constraint.RigidBodyA;
454 RigidBody colObj1 = constraint.RigidBodyB;
455
456 if (((colObj0 != null) && (colObj0.MergesSimulationIslands)) &&
457 ((colObj1 != null) && (colObj1.MergesSimulationIslands)))
458 {
459 if (colObj0.IsActive || colObj1.IsActive)
460 {
461
462 SimulationIslandManager.UnionFind.Unite((colObj0).IslandTag,
463 (colObj1).IslandTag);
464 }
465 }
466 }
467
468 //Store the island id in each body
469 SimulationIslandManager.StoreIslandActivationState(this);
470 }
471
472 //protected void SolveNonContactConstraints(ContactSolverInfo solverInfo)
473 //{
474 // //constraint preparation: building jacobians
475 // for (int i = 0; i < _constraints.Count; i++)
476 // {
477 // TypedConstraint constraint = _constraints[i];
478 // constraint.BuildJacobian();
479 // }
480
481 // //solve the regular non-contact constraints (point 2 point, hinge, generic d6)
482 // for (int g = 0; g < solverInfo.IterationsCount; g++)
483 // {
484 // for (int i = 0; i < _constraints.Count; i++)
485 // {
486 // TypedConstraint constraint = _constraints[i];
487 // constraint.SolveConstraint(solverInfo.TimeStep);
488 // }
489 // }
490 //}
491
492 //protected void SolveContactConstraints(ContactSolverInfo solverInfo)
493 //{
494 // InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, _debugDrawer);
495
496 // // solve all the contact points and contact friction
497 // _islandManager.BuildAndProcessIslands(Dispatcher, CollisionObjects, solverCallback);
498 //}
499
500 protected void SolveConstraints(ContactSolverInfo solverInfo)
501 {
502 //sorted version of all btTypedConstraint, based on islandId
503 List<TypedConstraint> sortedConstraints = new List<TypedConstraint>(ConstraintsCount);
504
505 for (int i = 0; i < ConstraintsCount; i++)
506 {
507 sortedConstraints.Add(_constraints[i]);
508 }
509
510 sortedConstraints.Sort(new Comparison<TypedConstraint>(TypedConstraint.SortConstraintOnIslandPredicate));
511 List<TypedConstraint> constraintsPtr = ConstraintsCount != 0 ? sortedConstraints : new List<TypedConstraint>();
512
513 InplaceSolverIslandCallback solverCallback = new InplaceSolverIslandCallback(solverInfo, _constraintSolver, constraintsPtr, _debugDrawer);
514
515 // solve all the constraints for this island
516 _islandManager.BuildAndProcessIslands(CollisionWorld.Dispatcher, CollisionWorld.CollisionObjects, solverCallback);
517 }
518
519 protected void UpdateActivationState(float timeStep)
520 {
521 for (int i = 0; i < CollisionObjects.Count; i++)
522 {
523 CollisionObject colObj = CollisionObjects[i];
524 RigidBody body = RigidBody.Upcast(colObj);
525 if (body != null)
526 {
527 body.UpdateDeactivation(timeStep);
528
529 if (body.WantsSleeping())
530 {
531 if (body.IsStaticOrKinematicObject)
532 {
533 body.ActivationState = ActivationState.IslandSleeping;
534 }
535 else
536 {
537 if (body.ActivationState == ActivationState.Active)
538 body.ActivationState = ActivationState.WantsDeactivation;
539 }
540 }
541 else
542 {
543 if (body.ActivationState != ActivationState.DisableDeactivation)
544 body.ActivationState = ActivationState.Active;
545 }
546 }
547 }
548 }
549
550 protected void UpdateVehicles(float timeStep)
551 {
552 for (int i = 0; i < _vehicles.Count; i++)
553 {
554 RaycastVehicle vehicle = _vehicles[i];
555 vehicle.updateVehicle(timeStep);
556 }
557 }
558
559 protected void StartProfiling(float timeStep) { }
560
561 protected virtual void InternalSingleStepSimulation(float timeStep)
562 {
563 StartProfiling(timeStep);
564
565 //update aabbs information
566 UpdateAabbs();
567
568 //apply gravity, predict motion
569 PredictUnconstraintMotion(timeStep);
570
571 DispatcherInfo dispatchInfo = DispatchInfo;
572 dispatchInfo.TimeStep = timeStep;
573 dispatchInfo.StepCount = 0;
574 dispatchInfo.DebugDraw = DebugDrawer;
575
576 //perform collision detection
577 PerformDiscreteCollisionDetection();
578
579 CalculateSimulationIslands();
580
581 SolverInfo.TimeStep = timeStep;
582
583 //solve contact and other joint constraints
584 SolveConstraints(SolverInfo);
585
586 //CallbackTriggers();
587
588 //integrate transforms
589 IntegrateTransforms(timeStep);
590
591 //update vehicle simulation
592 UpdateVehicles(timeStep);
593
594 UpdateActivationState(timeStep);
595 }
596
597 protected void SynchronizeMotionStates()
598 {
599 //debug vehicle wheels
600 {
601 //todo: iterate over awake simulation islands!
602 for (int i = 0; i < CollisionObjects.Count; i++)
603 {
604 CollisionObject colObj = CollisionObjects[i];
605 if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0)
606 {
607 Vector3 color = new Vector3(255f, 255f, 255f);
608 switch (colObj.ActivationState)
609 {
610 case ActivationState.Active:
611 color = new Vector3(255f, 255f, 255f); break;
612 case ActivationState.IslandSleeping:
613 color = new Vector3(0f, 255f, 0f); break;
614 case ActivationState.WantsDeactivation:
615 color = new Vector3(0f, 255f, 255f); break;
616 case ActivationState.DisableDeactivation:
617 color = new Vector3(255f, 0f, 0f); break;
618 case ActivationState.DisableSimulation:
619 color = new Vector3(255f, 255f, 0f); break;
620 default:
621 color = new Vector3(255f, 0f, 0f); break;
622 }
623
624 DebugDrawObject(colObj.WorldTransform, colObj.CollisionShape, color);
625 }
626 RigidBody body = RigidBody.Upcast(colObj);
627 if (body != null && body.MotionState != null && !body.IsStaticOrKinematicObject)
628 {
629 //if (body.ActivationState != ActivationState.IslandSleeping)
630 {
631 Matrix interpolatedTransform = new Matrix();
632 TransformUtil.IntegrateTransform(body.InterpolationWorldTransform,
633 body.InterpolationLinearVelocity, body.InterpolationAngularVelocity, _localTime, ref interpolatedTransform);
634 body.MotionState.SetWorldTransform(interpolatedTransform);
635 }
636 }
637 }
638 }
639
640 if (DebugDrawer != null && (DebugDrawer.DebugMode & DebugDrawModes.DrawWireframe) != 0)
641 {
642 for (int i = 0; i < _vehicles.Count; i++)
643 {
644 for (int v = 0; v < _vehicles[i].getNumWheels(); v++)
645 {
646 Vector3 wheelColor = new Vector3(0, 255, 255);
647 if (_vehicles[i].getWheelInfo(v).RaycastInfo.IsInContact)
648 {
649 wheelColor = new Vector3(0, 0, 255);
650 }
651 else
652 {
653 wheelColor = new Vector3(255, 0, 255);
654 }
655
656 //synchronize the wheels with the (interpolated) chassis worldtransform
657 _vehicles[i].updateWheelTransform(v, true);
658
659 Vector3 wheelPosWS = _vehicles[i].getWheelInfo(v).WorldTransform.Translation;
660
661 Vector3 axle = new Vector3(
662 MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 0, _vehicles[i].getRightAxis()),
663 MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 1, _vehicles[i].getRightAxis()),
664 MathHelper.GetElement(_vehicles[i].getWheelInfo(v).WorldTransform, 2, _vehicles[i].getRightAxis()));
665
666
667 //m_vehicles[i]->getWheelInfo(v).m_raycastInfo.m_wheelAxleWS
668 //debug wheels (cylinders)
669 _debugDrawer.DrawLine(wheelPosWS, wheelPosWS + axle, wheelColor);
670 _debugDrawer.DrawLine(wheelPosWS, _vehicles[i].getWheelInfo(v).RaycastInfo.ContactPointWS, wheelColor);
671 }
672 }
673 }
674 }
675
676 protected void SaveKinematicState(float timeStep)
677 {
678 for (int i = 0; i < CollisionObjects.Count; i++)
679 {
680 CollisionObject colObj = CollisionObjects[i];
681 RigidBody body = RigidBody.Upcast(colObj);
682 if (body != null)
683 {
684 if (body.ActivationState != ActivationState.IslandSleeping)
685 {
686 if (body.IsKinematicObject)
687 {
688 //to calculate velocities next frame
689 body.SaveKinematicState(timeStep);
690 }
691 }
692 }
693 }
694 }
695
696 internal class InplaceSolverIslandCallback : SimulationIslandManager.IIslandCallback
697 {
698 private ContactSolverInfo _solverInfo;
699 private IConstraintSolver _solver;
700 private IDebugDraw _debugDrawer;
701 private List<TypedConstraint> _sortedConstraints;
702
703 public InplaceSolverIslandCallback(
704 ContactSolverInfo solverInfo,
705 IConstraintSolver solver,
706 List<TypedConstraint> sortedConstraints,
707 IDebugDraw debugDrawer)
708 {
709 _solverInfo = solverInfo;
710 _solver = solver;
711 _sortedConstraints = sortedConstraints;
712 _debugDrawer = debugDrawer;
713 }
714
715 public ContactSolverInfo SolverInfo { get { return _solverInfo; } set { _solverInfo = value; } }
716 public IConstraintSolver Solver { get { return _solver; } set { _solver = value; } }
717 public List<TypedConstraint> Constraints { get { return _sortedConstraints; } set { _sortedConstraints = value; } }
718 public IDebugDraw DebugDrawer { get { return _debugDrawer; } set { _debugDrawer = value; } }
719
720 public void ProcessIsland(List<CollisionObject> bodies, List<PersistentManifold> manifolds, int numManifolds, int islandID)
721 {
722 //also add all non-contact constraints/joints for this island
723 List<TypedConstraint> startConstraint = new List<TypedConstraint>();
724 int numCurConstraints = 0;
725 int startIndex = 0;
726 int i;
727
728 //find the first constraint for this island
729 for (i = 0; i < _sortedConstraints.Count; i++)
730 {
731 if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID)
732 {
733 //startConstraint = &m_sortedConstraints[i];
734 startIndex = i;
735 break;
736 }
737 }
738 //count the number of constraints in this island
739 for (; i < _sortedConstraints.Count; i++)
740 {
741 if (TypedConstraint.GetConstraintIslandId(_sortedConstraints[i]) == islandID)
742 {
743 numCurConstraints++;
744 }
745 }
746
747 for (i = startIndex; i < startIndex + numCurConstraints; i++)
748 {
749 startConstraint.Add(_sortedConstraints[i]);
750 }
751
752 _solver.SolveGroup(bodies, manifolds, numManifolds, startConstraint, _solverInfo, _debugDrawer);
753 }
754 }
755 }
756
757 internal class DebugDrawCallback : ITriangleIndexCallback, ITriangleCallback
758 {
759 private IDebugDraw _debugDrawer;
760 private Vector3 _color;
761 private Matrix _worldTrans;
762
763 public DebugDrawCallback(IDebugDraw debugDrawer, Matrix worldTrans, Vector3 color)
764 {
765 _debugDrawer = debugDrawer;
766 _worldTrans = worldTrans;
767 _color = color;
768 }
769
770 public void ProcessTriangleIndex(Vector3[] triangle, int partId, int triangleIndex)
771 {
772 ProcessTriangle(triangle, partId, triangleIndex);
773 }
774
775 #region ITriangleCallback Members
776
777 public void ProcessTriangle(Vector3[] triangle, int partID, int triangleIndex)
778 {
779 Vector3 wv0, wv1, wv2;
780 wv0 = Vector3.TransformNormal(triangle[0], _worldTrans);
781 wv1 = Vector3.TransformNormal(triangle[1], _worldTrans);
782 wv2 = Vector3.TransformNormal(triangle[2], _worldTrans);
783 _debugDrawer.DrawLine(wv0, wv1, _color);
784 _debugDrawer.DrawLine(wv1, wv2, _color);
785 _debugDrawer.DrawLine(wv2, wv0, _color);
786 }
787
788 #endregion
789 }
790} \ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs
new file mode 100644
index 0000000..647c8ec
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/DynamicsWorld.cs
@@ -0,0 +1,59 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX.Dynamics
28{
29 public abstract class DynamicsWorld : CollisionWorld
30 {
31 public DynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache)
32 : base(dispatcher, pairCache) { }
33
34 //once a rigidbody is added to the dynamics world, it will get this gravity assigned
35 //existing rigidbodies in the world get gravity assigned too, during this method
36 public abstract Vector3 Gravity { set; }
37 public abstract IConstraintSolver ConstraintSolver { set; }
38 public virtual int ConstraintsCount { get { return 0; } }
39 public abstract IDebugDraw DebugDrawer { get; set; }
40
41 //stepSimulation proceeds the simulation over timeStep units
42 public abstract void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep);
43
44 public void StepSimulation(float timeStep)
45 {
46 StepSimulation(timeStep, 1, 1f / 60f);
47 }
48
49 public abstract void UpdateAabbs();
50
51 public virtual void AddConstraint(TypedConstraint constraint) { }
52 public virtual void RemoveConstraint(TypedConstraint constraint) { }
53
54 public abstract void AddRigidBody(RigidBody body);
55 public abstract void RemoveRigidBody(RigidBody body);
56
57 public virtual TypedConstraint GetConstraint(int index) { return null; }
58 }
59}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs
new file mode 100644
index 0000000..ec14daf
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/RigidBody.cs
@@ -0,0 +1,447 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX.Dynamics
28{
29 public class RigidBody : CollisionObject
30 {
31 private static float _linearAirDamping = 1;
32 //'temporarily' global variables
33 private static float _rigidBodyDeactivationTime = 2;
34 private static bool _disableDeactivation = false;
35
36 private static float _linearSleepingThreshold = 0.8f;
37 private static float _angularSleepingThreshold = 1.0f;
38 private static int _uniqueId = 0;
39
40 private Matrix _invInertiaTensorWorld;
41 private Vector3 _linearVelocity;
42 private Vector3 _angularVelocity;
43 private float _inverseMass;
44 private float _angularFactor;
45
46 private Vector3 _gravity;
47 private Vector3 _invInertiaLocal;
48 private Vector3 _totalForce;
49 private Vector3 _totalTorque;
50
51 private float _linearDamping;
52 private float _angularDamping;
53
54 //m_optionalMotionState allows to automatic synchronize the world transform for active objects
55 private MotionState _optionalMotionState;
56
57 //for experimental overriding of friction/contact solver func
58 private ContactSolverType _contactSolverType;
59 private ContactSolverType _frictionSolverType;
60
61 private int _debugBodyId;
62
63 //Bullet 2.20b has experimental damping code to reduce jitter just before objects fall asleep/deactivate
64 //doesn't work very well yet (value 0 disabled this damping)
65 //note there this influences deactivation thresholds!
66 private float _clippedAngvelThresholdSqr = 0.01f;
67 private float _clippedLinearThresholdSqr = 0.01f;
68
69 private float _jitterVelocityDampingFactor = 0.7f;
70
71 public RigidBody(float mass, MotionState motionState, CollisionShape collisionShape, Vector3 localInertia, float linearDamping, float angularDamping, float friction, float restitution)
72 {
73 _optionalMotionState = motionState;
74 _angularFactor = 1;
75 _angularDamping = 0.5f;
76
77 if (motionState != null)
78 {
79 motionState.GetWorldTransform(out _worldTransform);
80 }
81 else
82 {
83 WorldTransform = Matrix.Identity;
84 }
85
86 InterpolationWorldTransform = WorldTransform;
87 InterpolationLinearVelocity = new Vector3();
88 InterpolationAngularVelocity = new Vector3();
89
90 //moved to btCollisionObject
91 Friction = friction;
92 Restitution = restitution;
93
94 CollisionShape = collisionShape;
95 _debugBodyId = UniqueID++;
96
97 //m_internalOwner is to allow upcasting from collision object to rigid body
98 Owner = this;
99
100 SetMassProps(mass, localInertia);
101 SetDamping(linearDamping, angularDamping);
102 UpdateInertiaTensor();
103 }
104
105 public int DebugBodyID { get { return _debugBodyId; } set { _debugBodyId = value; } }
106
107 public ContactSolverType ContactSolverType { get { return _contactSolverType; } set { _contactSolverType = value; } }
108 public ContactSolverType FrictionSolverType { get { return _frictionSolverType; } set { _frictionSolverType = value; } }
109
110 public float AngularFactor { get { return _angularFactor; } set { _angularFactor = value; } }
111
112 //is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase?
113 public bool IsInWorld { get { return Broadphase != null; } }
114
115 public Vector3 Gravity
116 {
117 get { return _gravity; }
118 set
119 {
120 if (_inverseMass != 0.0f)
121 {
122 _gravity = value * (1.0f / _inverseMass);
123 }
124 }
125 }
126 public Matrix InvInertiaTensorWorld { get { return _invInertiaTensorWorld; } }
127 public float InverseMass { get { return _inverseMass; } }
128 public Vector3 InvInertiaDiagLocal { get { return _invInertiaLocal; } set { _invInertiaLocal = value; } }
129 public Vector3 CenterOfMassPosition { get { return WorldTransform.Translation; } }
130 public Quaternion Orientation { get { return Quaternion.CreateFromRotationMatrix(WorldTransform); } }
131 public Matrix CenterOfMassTransform
132 {
133 get { return WorldTransform; }
134 set
135 {
136 InterpolationWorldTransform = value;
137 InterpolationLinearVelocity = LinearVelocity;
138 InterpolationAngularVelocity = AngularVelocity;
139 WorldTransform = value;
140 UpdateInertiaTensor();
141 }
142 }
143
144 public Vector3 LinearVelocity
145 {
146 get { return _linearVelocity; }
147 set
148 {
149 if (CollisionFlags == CollisionOptions.StaticObject)
150 throw new BulletException("Static objects can't have linear velocity!");
151 _linearVelocity = value;
152 }
153 }
154
155 public Vector3 AngularVelocity
156 {
157 get { return _angularVelocity; }
158 set
159 {
160 if (CollisionFlags == CollisionOptions.StaticObject)
161 throw new BulletException("Static objects can't have angular velocity!");
162 _angularVelocity = value;
163 }
164 }
165
166 //MotionState allows to automatic synchronize the world transform for active objects
167 public MotionState MotionState
168 {
169 get { return _optionalMotionState; }
170 set
171 {
172 _optionalMotionState = value;
173 if (_optionalMotionState != null)
174 value.GetWorldTransform(out _worldTransform);
175 }
176 }
177
178 public static float LinearAirDamping { get { return _linearAirDamping; } set { _linearAirDamping = value; } }
179 public static float RigidBodyDeactivationTime { get { return _rigidBodyDeactivationTime; } set { _rigidBodyDeactivationTime = value; } }
180 public static bool DisableDeactivation { get { return _disableDeactivation; } set { _disableDeactivation = value; } }
181 public static float LinearSleepingThreshold { get { return _linearSleepingThreshold; } set { _linearSleepingThreshold = value; } }
182 public static float AngularSleepingThreshold { get { return _angularSleepingThreshold; } set { _angularSleepingThreshold = value; } }
183 public static int UniqueID { get { return _uniqueId; } set { _uniqueId = value; } }
184
185 public void ProceedToTransform(Matrix newTrans)
186 {
187 CenterOfMassTransform = newTrans;
188 }
189
190 //to keep collision detection and dynamics separate we don't store a rigidbody pointer
191 //but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast
192 public static RigidBody Upcast(CollisionObject colObj)
193 {
194 return colObj.Owner as RigidBody;
195 }
196
197 // continuous collision detection needs prediction
198 public void PredictIntegratedTransform(float step, ref Matrix predictedTransform)
199 {
200 if ((_angularVelocity.LengthSquared() < _clippedAngvelThresholdSqr) &&
201 (_linearVelocity.LengthSquared() < _clippedLinearThresholdSqr))
202 {
203 _angularVelocity *= _jitterVelocityDampingFactor;
204 _linearVelocity *= _jitterVelocityDampingFactor;
205 }
206
207 TransformUtil.IntegrateTransform(WorldTransform, _linearVelocity, _angularVelocity, step, ref predictedTransform);
208 }
209
210 public void SaveKinematicState(float step)
211 {
212 //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
213 if (step != 0)
214 {
215 //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
216 if (MotionState != null)
217 MotionState.GetWorldTransform(out _worldTransform);
218
219 TransformUtil.CalculateVelocity(InterpolationWorldTransform, WorldTransform, step, ref _linearVelocity, ref _angularVelocity);
220 InterpolationLinearVelocity = _linearVelocity;
221 InterpolationAngularVelocity = _angularVelocity;
222 InterpolationWorldTransform = WorldTransform;
223 }
224 }
225
226 public void ApplyForces(float step)
227 {
228 if (IsStaticOrKinematicObject)
229 return;
230
231 ApplyCentralForce(_gravity);
232
233 _linearVelocity *= (1 - step * LinearAirDamping * _linearDamping) < 0.0f ? 0.0f : (1.0f < (1 - step * LinearAirDamping * _linearDamping) ? 1.0f : (1 - step * LinearAirDamping * _linearDamping));
234 _angularVelocity *= (1 - step * _angularDamping) < 0.0f ? 0.0f : (1.0f < (1 - step * _angularDamping) ? 1.0f : (1 - step * _angularDamping));
235
236 float speed = _linearVelocity.Length();
237 if (speed < _linearDamping)
238 {
239 float dampVel = 0.005f;
240 if (speed > dampVel)
241 {
242 Vector3 dir = _linearVelocity;
243 dir.Normalize();
244 _linearVelocity -= dir * dampVel;
245 }
246 else
247 {
248 _linearVelocity = new Vector3();
249 }
250 }
251
252 float angSpeed = _angularVelocity.Length();
253 if (angSpeed < _angularDamping)
254 {
255 float angDampVel = 0.005f;
256 if (angSpeed > angDampVel)
257 {
258 Vector3 dir = _angularVelocity;
259 dir.Normalize();
260 _angularVelocity -= dir * angDampVel;
261 }
262 else
263 {
264 _angularVelocity = new Vector3();
265 }
266 }
267 }
268
269 public void SetDamping(float linDamping, float angDamping)
270 {
271 _linearDamping = linDamping < 0.0f ? 0.0f : (1.0f < linDamping ? 1.0f : linDamping);
272 _angularDamping = angDamping < 0.0f ? 0.0f : (1.0f < angDamping ? 1.0f : angDamping);
273 }
274
275 public void SetMassProps(float mass, Vector3 inertia)
276 {
277 if (mass == 0)
278 {
279 CollisionFlags |= CollisionOptions.StaticObject;
280 _inverseMass = 0;
281 }
282 else
283 {
284 CollisionFlags &= (~CollisionOptions.StaticObject);
285 _inverseMass = 1.0f / mass;
286 }
287
288 _invInertiaLocal = new Vector3(inertia.X != 0.0f ? 1.0f / inertia.X : 0.0f,
289 inertia.Y != 0.0f ? 1.0f / inertia.Y : 0.0f,
290 inertia.Z != 0.0f ? 1.0f / inertia.Z : 0.0f);
291 }
292
293 public void IntegrateVelocities(float step)
294 {
295 if (IsStaticOrKinematicObject)
296 return;
297
298 _linearVelocity += _totalForce * (_inverseMass * step);
299 _angularVelocity += Vector3.TransformNormal(_totalTorque, _invInertiaTensorWorld) * step;
300
301 //float MAX_ANGVEL = MonoXnaCompactMaths.MathHelper.PiOver2;
302 float MAX_ANGVEL = (float)Math.PI / 2.0f;
303 /// clamp angular velocity. collision calculations will fail on higher angular velocities
304 float angvel = _angularVelocity.Length();
305 if (angvel * step > MAX_ANGVEL)
306 {
307 _angularVelocity *= (MAX_ANGVEL / step) / angvel;
308 }
309
310 ClearForces();
311 }
312
313 public void ApplyCentralForce(Vector3 force)
314 {
315 _totalForce += force;
316 }
317
318 public void ApplyTorque(Vector3 torque)
319 {
320 _totalTorque += torque;
321 }
322
323 public void ApplyForce(Vector3 force, Vector3 rel_pos)
324 {
325 ApplyCentralForce(force);
326 ApplyTorque(Vector3.Cross(rel_pos, force));
327 }
328
329 public void ApplyCentralImpulse(Vector3 impulse)
330 {
331 _linearVelocity += impulse * _inverseMass;
332 }
333
334 public void ApplyTorqueImpulse(Vector3 torque)
335 {
336 _angularVelocity += Vector3.TransformNormal(torque, _invInertiaTensorWorld);
337 }
338
339 public void ApplyImpulse(Vector3 impulse, Vector3 rel_pos)
340 {
341 if (_inverseMass != 0)
342 {
343 ApplyCentralImpulse(impulse);
344 if (_angularFactor != 0)
345 ApplyTorqueImpulse(Vector3.Cross(rel_pos, impulse) * _angularFactor);
346 }
347 }
348
349 public void InternalApplyImpulse(Vector3 linearComponent, Vector3 angularComponent, float impulseMagnitude)
350 {
351 if (_inverseMass != 0)
352 {
353 _linearVelocity += linearComponent * impulseMagnitude;
354 if (_angularFactor != 0)
355 _angularVelocity += angularComponent * impulseMagnitude * _angularFactor;
356 }
357 }
358
359 public void ClearForces()
360 {
361 _totalForce = new Vector3();
362 _totalTorque = new Vector3();
363 }
364
365 public void UpdateInertiaTensor()
366 {
367 Matrix temp = WorldTransform;
368 temp.Translation = Vector3.Zero;
369 _invInertiaTensorWorld = MatrixOperations.Multiply(MatrixOperations.Scaled(WorldTransform, _invInertiaLocal), Matrix.Transpose(temp));
370 }
371
372 public Vector3 GetVelocityInLocalPoint(Vector3 relPos)
373 {
374 //we also calculate lin/ang velocity for kinematic objects
375 return _linearVelocity + Vector3.Cross(_angularVelocity, relPos);
376
377 //for kinematic objects, we could also use use:
378 // return (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
379 }
380
381 public void Translate(Vector3 v)
382 {
383 Matrix m = WorldTransform;
384 m.Translation += v;
385 WorldTransform = m;
386 }
387
388 public void GetAabb(out Vector3 aabbMin, out Vector3 aabbMax)
389 {
390 CollisionShape.GetAabb(WorldTransform, out aabbMin, out aabbMax);
391 }
392
393 public float ComputeImpulseDenominator(Vector3 pos, Vector3 normal)
394 {
395 Vector3 r0 = pos - CenterOfMassPosition;
396 Vector3 c0 = Vector3.Cross(r0, normal);
397 Vector3 vec = Vector3.Cross(Vector3.TransformNormal(c0, InvInertiaTensorWorld), r0);
398
399 return _inverseMass + Vector3.Dot(normal, vec);
400
401 }
402
403 public float ComputeAngularImpulseDenominator(Vector3 axis)
404 {
405 Vector3 vec = Vector3.TransformNormal(axis, InvInertiaTensorWorld);
406 return Vector3.Dot(axis, vec);
407 }
408
409 public void UpdateDeactivation(float timeStep)
410 {
411 if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.DisableDeactivation))
412 return;
413
414 if ((LinearVelocity.LengthSquared() < LinearSleepingThreshold * LinearSleepingThreshold) &&
415 (AngularVelocity.LengthSquared() < AngularSleepingThreshold * AngularSleepingThreshold))
416 {
417 DeactivationTime += timeStep;
418 }
419 else
420 {
421 DeactivationTime = 0;
422 ActivationState = ActivationState.Nothing;
423 }
424
425 }
426
427 public bool WantsSleeping()
428 {
429
430 if (ActivationState == ActivationState.DisableDeactivation)
431 return false;
432
433 //disable deactivation
434 if (DisableDeactivation || (RigidBodyDeactivationTime == 0))
435 return false;
436
437 if ((ActivationState == ActivationState.IslandSleeping) || (ActivationState == ActivationState.WantsDeactivation))
438 return true;
439
440 if (DeactivationTime > RigidBodyDeactivationTime)
441 {
442 return true;
443 }
444 return false;
445 }
446 }
447}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs
new file mode 100644
index 0000000..aa91a03
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/SimpleDynamicsWorld.cs
@@ -0,0 +1,211 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX.Dynamics
28{
29 public class SimpleDynamicsWorld : DynamicsWorld
30 {
31 private IConstraintSolver _constraintSolver;
32 private bool _ownsConstraintSolver;
33 private Vector3 _gravity;
34 private IDebugDraw _debugDrawer;
35
36 /// <summary>
37 /// this btSimpleDynamicsWorld constructor creates dispatcher, broadphase pairCache and constraintSolver
38 /// </summary>
39 /// <param name="dispatcher"></param>
40 /// <param name="pairCache"></param>
41 /// <param name="constraintSolver"></param>
42 public SimpleDynamicsWorld(IDispatcher dispatcher, OverlappingPairCache pairCache, IConstraintSolver constraintSolver)
43 : base(dispatcher, pairCache)
44 {
45 _constraintSolver = constraintSolver;
46 _ownsConstraintSolver = false;
47 _gravity = new Vector3(0, 0, -10);
48 }
49
50 public override Vector3 Gravity
51 {
52 set
53 {
54 _gravity = value;
55 for (int i = 0; i < CollisionObjects.Count; i++)
56 {
57 CollisionObject colObj = CollisionObjects[i];
58 RigidBody body = RigidBody.Upcast(colObj);
59 if (body != null)
60 {
61 body.Gravity = value;
62 }
63 }
64 }
65 }
66
67 public override IConstraintSolver ConstraintSolver
68 {
69 set
70 {
71 _ownsConstraintSolver = false;
72 _constraintSolver = value;
73 }
74 }
75
76 public override IDebugDraw DebugDrawer
77 {
78 get
79 {
80 return _debugDrawer;
81 }
82 set
83 {
84 _debugDrawer = value;
85 }
86 }
87
88 public override void StepSimulation(float timeStep, int numSubsteps, float fixedTimeStep)
89 {
90 //apply gravity, predict motion
91 PredictUnconstraintMotion(timeStep);
92
93 DispatcherInfo dispatchInfo = new DispatcherInfo();
94 dispatchInfo.TimeStep = timeStep;
95 dispatchInfo.StepCount = 0;
96 dispatchInfo.DebugDraw = DebugDrawer;
97 //perform collision detection
98 PerformDiscreteCollisionDetection();
99
100 //solve contact constraints
101 int numManifolds = Dispatcher.ManifoldCount;
102 if (numManifolds != 0)
103 {
104
105 List<PersistentManifold> manifolds = (Dispatcher as CollisionDispatcher).Manifolds;
106 //int numManifolds = m_dispatcher1.GetNumManifolds();
107 ContactSolverInfo infoGlobal = new ContactSolverInfo();
108 infoGlobal.TimeStep = timeStep;
109
110 _constraintSolver.SolveGroup(new List<CollisionObject>(), manifolds, manifolds.Count, new List<TypedConstraint>(), infoGlobal, _debugDrawer);
111 }
112 //integrate transforms
113 IntegrateTransforms(timeStep);
114
115 UpdateAabbs();
116
117 SynchronizeMotionStates();
118 }
119
120 public override void UpdateAabbs()
121 {
122 for (int i = 0; i < CollisionObjects.Count; i++)
123 {
124 CollisionObject colObj = CollisionObjects[i];
125 RigidBody body = RigidBody.Upcast(colObj);
126 if (body != null)
127 {
128 if (body.IsActive && (!body.IsStaticObject))
129 {
130 Vector3 minAabb, maxAabb;
131 colObj.CollisionShape.GetAabb(colObj.WorldTransform, out minAabb, out maxAabb);
132 IBroadphase bp = Broadphase;
133 bp.SetAabb(body.Broadphase, minAabb, maxAabb);
134 }
135 }
136 }
137 }
138
139 public override void AddRigidBody(RigidBody body)
140 {
141 body.Gravity = _gravity;
142
143 if (body.CollisionShape != null)
144 {
145 AddCollisionObject(body);
146 }
147 }
148
149 public override void RemoveRigidBody(RigidBody body)
150 {
151 RemoveCollisionObject(body);
152 }
153
154 public void SynchronizeMotionStates()
155 {
156 for (int i = 0; i < CollisionObjects.Count; i++)
157 {
158 CollisionObject colObj = CollisionObjects[i];
159 RigidBody body = RigidBody.Upcast(colObj);
160 if (body != null && body.MotionState != null)
161 {
162 if (body.ActivationState != ActivationState.IslandSleeping)
163 {
164 body.MotionState.SetWorldTransform(body.WorldTransform);
165 }
166 }
167 }
168 }
169
170 protected void PredictUnconstraintMotion(float timeStep)
171 {
172 for (int i = 0; i < CollisionObjects.Count; i++)
173 {
174 CollisionObject colObj = CollisionObjects[i];
175 RigidBody body = RigidBody.Upcast(colObj);
176 if (body != null)
177 {
178 if (!body.IsStaticObject)
179 {
180 if (body.IsActive)
181 {
182 body.ApplyForces(timeStep);
183 body.IntegrateVelocities(timeStep);
184 Matrix temp = body.InterpolationWorldTransform;
185 body.PredictIntegratedTransform(timeStep, ref temp);
186 body.InterpolationWorldTransform = temp;
187 }
188 }
189 }
190 }
191 }
192
193 protected void IntegrateTransforms(float timeStep)
194 {
195 Matrix predictedTrans = Matrix.Identity;
196 for (int i = 0; i < CollisionObjects.Count; i++)
197 {
198 CollisionObject colObj = CollisionObjects[i];
199 RigidBody body = RigidBody.Upcast(colObj);
200 if (body != null)
201 {
202 if (body.IsActive && (!body.IsStaticObject))
203 {
204 body.PredictIntegratedTransform(timeStep, ref predictedTrans);
205 body.ProceedToTransform(predictedTrans);
206 }
207 }
208 }
209 }
210 }
211}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs
new file mode 100644
index 0000000..90db9ee
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/RaycastVehicle.cs
@@ -0,0 +1,94 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX.Dynamics
28{
29 public class RaycastVehicle : TypedConstraint
30 {
31 public override void BuildJacobian()
32 {
33 throw new Exception("The method or operation is not implemented.");
34 }
35
36 public override void SolveConstraint(float timeStep)
37 {
38 throw new Exception("The method or operation is not implemented.");
39 }
40
41 public int getNumWheels()
42 {
43 throw new Exception("The method or operation is not implemented.");
44 }
45
46 public WheelInfo getWheelInfo(int v)
47 {
48 throw new Exception("The method or operation is not implemented.");
49 }
50
51 public void updateWheelTransform(int v, bool p)
52 {
53 throw new Exception("The method or operation is not implemented.");
54 }
55
56 public int getRightAxis()
57 {
58 throw new Exception("The method or operation is not implemented.");
59 }
60
61 public void updateVehicle(float timeStep)
62 {
63 throw new Exception("The method or operation is not implemented.");
64 }
65 }
66
67 public class DefaultVehicleRaycaster : IVehicleRaycaster
68 {
69 DynamicsWorld _dynamicsWorld;
70
71 public DefaultVehicleRaycaster(DynamicsWorld world)
72 {
73 _dynamicsWorld = world;
74 }
75
76 public object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result)
77 {
78 CollisionWorld.ClosestRayResultCallback rayCallback = new CollisionWorld.ClosestRayResultCallback(from, to);
79 _dynamicsWorld.RayTest(from, to, rayCallback);
80
81 result = new VehicleRaycasterResult();
82
83 if (!rayCallback.HasHit) return 0;
84 RigidBody body = RigidBody.Upcast(rayCallback.CollisionObject);
85 if (body == null) return 0;
86
87 result.HitPointInWorld = rayCallback.HitPointWorld;
88 result.HitNormalInWorld = rayCallback.HitNormalWorld;
89 result.HitNormalInWorld.Normalize();
90 result.DistFraction = rayCallback.ClosestHitFraction;
91 return body;
92 }
93 }
94} \ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/VehicleRaycaster.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/VehicleRaycaster.cs
new file mode 100644
index 0000000..86829e3
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/VehicleRaycaster.cs
@@ -0,0 +1,49 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX.Dynamics
28{
29 public interface IVehicleRaycaster
30 {
31 object CastRay(Vector3 from, Vector3 to, out VehicleRaycasterResult result);
32 }
33
34 public class VehicleRaycasterResult
35 {
36 private Single _distFraction;
37 private Vector3 _hitNormalInWorld;
38 private Vector3 _hitPointInWorld;
39
40 public VehicleRaycasterResult()
41 {
42 _distFraction = -1;
43 }
44
45 public float DistFraction { get { return _distFraction; } set { _distFraction = value; } }
46 public Vector3 HitNormalInWorld { get { return _hitNormalInWorld; } set { _hitNormalInWorld = value; } }
47 public Vector3 HitPointInWorld { get { return _hitPointInWorld; } set { _hitPointInWorld = value; } }
48 }
49}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs
new file mode 100644
index 0000000..d4a94f3
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Dynamics/Vehicle/WheelInfo.cs
@@ -0,0 +1,529 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX.Dynamics
28{
29 public struct WheelInfoConstructionInfo
30 {
31 private Vector3 _chassicConnectionCS;
32 private Vector3 _wheelDirectionCS;
33 private Vector3 _wheelAxisCS;
34
35 private Single _suspensionRestLength;
36 private Single _maxSuspensionTravelCm;
37 private Single _wheelRadius;
38 private Single _suspensionStiffness;
39 private Single _wheelsDampingCompression;
40 private Single _wheelsDampingRelaxation;
41 private Single _frictionSlip;
42
43 private Boolean _isFrontWheel;
44
45 #region Basic Properties
46 public Vector3 ChassicConnectionCS
47 {
48 get { return _chassicConnectionCS; }
49 set { _chassicConnectionCS = value; }
50 }
51
52 public Vector3 WheelDirectionCS
53 {
54 get { return _wheelDirectionCS; }
55 set { _wheelDirectionCS = value; }
56 }
57
58 public Vector3 WheelAxleCS
59 {
60 get { return _wheelAxisCS; }
61 set { _wheelAxisCS = value; }
62 }
63
64
65 public Single SuspensionRestLength
66 {
67 get { return _suspensionRestLength; }
68 set { _suspensionRestLength = value; }
69 }
70
71 public Single MaxSuspensionTravelCm
72 {
73 get { return _maxSuspensionTravelCm; }
74 set { _maxSuspensionTravelCm = value; }
75 }
76
77 public Single WheelRadius
78 {
79 get { return _wheelRadius; }
80 set { _wheelRadius = value; }
81 }
82
83
84 public Single SuspensionStiffness
85 {
86 get { return _suspensionStiffness; }
87 set { _suspensionStiffness = value; }
88 }
89
90 public Single WheelsDampingCompression
91 {
92 get { return _wheelsDampingCompression; }
93 set { _wheelsDampingCompression = value; }
94 }
95
96 public Single WheelsDampingRelaxation
97 {
98 get { return _wheelsDampingRelaxation; }
99 set { _wheelsDampingRelaxation = value; }
100 }
101
102 public Single FrictionSlip
103 {
104 get { return _frictionSlip; }
105 set { _frictionSlip = value; }
106 }
107
108
109 public Boolean IsFrontWheel
110 {
111 get { return _isFrontWheel; }
112 set { _isFrontWheel = value; }
113 }
114 #endregion
115 }
116
117 public struct RaycastInfo
118 {
119 private Vector3 _contractNormalWS;
120 private Vector3 _contractPointWS;
121
122 private Vector3 _hardPointWS;
123 private Vector3 _wheelDirectionWS;
124 private Vector3 _wheelAxleWS;
125
126 private Single _suspensionLength;
127 private Boolean _isInContract;
128
129 #region Basic Properties
130 public Single SuspensionLength
131 {
132 get { return _suspensionLength; }
133 set { _suspensionLength = value; }
134 }
135
136 public Boolean IsInContact
137 {
138 get { return _isInContract; }
139 set { _isInContract = value; }
140 }
141
142 public Vector3 ContactNormalWS
143 {
144 get { return _contractNormalWS; }
145 set { _contractNormalWS = value; }
146 }
147
148 public Vector3 ContactPointWS
149 {
150 get { return _contractPointWS; }
151 set { _contractPointWS = value; }
152 }
153
154 public Vector3 HardPointWS
155 {
156 get { return _hardPointWS; }
157 set { _hardPointWS = value; }
158 }
159
160 public Vector3 WheelDirectionWS
161 {
162 get { return _wheelDirectionWS; }
163 set { _wheelDirectionWS = value; }
164 }
165
166 public Vector3 WheelAxleWS
167 {
168 get { return _wheelAxleWS; }
169 set { _wheelAxleWS = value; }
170 }
171 #endregion
172 }
173
174 public struct WheelInfo
175 {
176 private RaycastInfo _raycastInfo;
177
178 private Matrix _worldTransform;
179
180 private Vector3 _chassicConnectionPointCS;
181 private Vector3 _wheelDirectionCS;
182 private Vector3 _wheelAxleCS;
183
184 private Single _suspensionRestLength;
185 private Single _maxSuspensionTravelCm;
186
187 private Single _wheelsRadius;
188 private Single _rollInfluence;
189 private Single _suspensionStiffness;
190 private Single _wheelsDampingCompression;
191 private Single _wheelsDampingRelaxation;
192 private Single _frictionSlip;
193 private Single _steering;
194 private Single _rotation;
195 private Single _deltaRotation;
196
197 private Single _engineForce;
198 private Single _brake;
199 private Boolean _isFrontWheel;
200
201
202 private Single _clippedInvContactDotSuspension;
203 private Single _skidInfo;
204 private Single _wheelsSuspensionForce;
205 private Single _suspensionRelativeVelocity;
206 //can be used to store pointer to sync transforms...
207 private object _clientInfo;
208
209 #region Constructor
210 public WheelInfo(WheelInfoConstructionInfo constructionInfo)
211 {
212 _suspensionRestLength = constructionInfo.SuspensionRestLength;
213 _maxSuspensionTravelCm = constructionInfo.MaxSuspensionTravelCm;
214
215 _wheelsRadius = constructionInfo.WheelRadius;
216 _wheelsDampingCompression = constructionInfo.WheelsDampingCompression;
217 _wheelsDampingRelaxation = constructionInfo.WheelsDampingRelaxation;
218 _wheelDirectionCS = constructionInfo.WheelDirectionCS;
219
220 _suspensionStiffness = constructionInfo.SuspensionStiffness;
221 _chassicConnectionPointCS = constructionInfo.ChassicConnectionCS;
222
223 _wheelAxleCS = constructionInfo.WheelAxleCS;
224 _frictionSlip = constructionInfo.FrictionSlip;
225
226 _clippedInvContactDotSuspension = 0;
227 _suspensionRelativeVelocity = 0;
228 _wheelsSuspensionForce = 0;
229 _skidInfo = 0;
230
231 _steering = 0;
232 _engineForce = 0;
233 _rotation = 0;
234 _rotation = 0;
235 _deltaRotation = 0;
236 _brake = 0;
237 _rollInfluence = 0.1f;
238 _brake = 0;
239 _rollInfluence = 0.1f;
240
241 _isFrontWheel = constructionInfo.IsFrontWheel;
242
243 _raycastInfo = default(RaycastInfo);
244 _worldTransform = default(Matrix);
245 _clientInfo = null;
246 }
247 #endregion
248
249 #region BasicProperties
250 public object ClientInfo { get { return _clientInfo; } set { _clientInfo = value; } }
251
252 public RaycastInfo RaycastInfo
253 {
254 get { return _raycastInfo; }
255 set { _raycastInfo = value; }
256 }
257
258 public Matrix WorldTransform
259 {
260 get { return _worldTransform; }
261 set { _worldTransform = value; }
262 }
263
264 public Vector3 ChassicConnectionPointCS
265 {
266 get { return _chassicConnectionPointCS; }
267 set { _chassicConnectionPointCS = value; }
268 }
269 public Vector3 WheelDirectionCS
270 {
271 get { return _wheelDirectionCS; }
272 set { _wheelDirectionCS = value; }
273 }
274 public Vector3 WheelAxleCS
275 {
276 get { return _wheelAxleCS; }
277 set { _wheelAxleCS = value; }
278 }
279
280 public Single SuspensionRestLength
281 {
282 get { return _suspensionRestLength; }
283 set { _suspensionRestLength = value; }
284 }
285
286
287 public Single MaxSuspensionTravelCm
288 {
289 get { return _maxSuspensionTravelCm; }
290 set { _maxSuspensionTravelCm = value; }
291 }
292
293 public Single WheelsRadius
294 {
295 get { return _wheelsRadius; }
296 set { _wheelsRadius = value; }
297 }
298
299 public Single SuspensionStiffness
300 {
301 get { return _suspensionStiffness; }
302 set { _suspensionStiffness = value; }
303 }
304
305 public Single WheelsDampingCompression
306 {
307 get { return _wheelsDampingCompression; }
308 set { _wheelsDampingCompression = value; }
309 }
310
311 public Single WheelsDampingRelaxation
312 {
313 get { return _wheelsDampingRelaxation; }
314 set { _wheelsDampingRelaxation = value; }
315 }
316
317 public Single FrictionSlip
318 {
319 get { return _frictionSlip; }
320 set { _frictionSlip = value; }
321 }
322
323 public Single Steering
324 {
325 get { return _steering; }
326 set { _steering = value; }
327 }
328
329 public Single Rotation
330 {
331 get { return _rotation; }
332 set { _rotation = value; }
333 }
334
335 public Single DeltaRotation
336 {
337 get { return _deltaRotation; }
338 set { _deltaRotation = value; }
339 }
340
341 public Single RollInfluence
342 {
343 get { return _rollInfluence; }
344 set { _rollInfluence = value; }
345 }
346
347 public Single EngineForce
348 {
349 get { return _engineForce; }
350 set { _engineForce = value; }
351 }
352
353 public Single Brake
354 {
355 get { return _brake; }
356 set { _brake = value; }
357 }
358
359 public Boolean IsFrontWheel
360 {
361 get { return _isFrontWheel; }
362 set { _isFrontWheel = value; }
363 }
364
365 public Single ClippedInvContactDotSuspension
366 {
367 get { return _clippedInvContactDotSuspension; }
368 set { _clippedInvContactDotSuspension = value; }
369 }
370
371 public Single SuspensionRelativeVelocity
372 {
373 get { return _suspensionRelativeVelocity; }
374 set { _suspensionRelativeVelocity = value; }
375 }
376
377 public Single WheelsSuspensionForce
378 {
379 get { return _wheelsSuspensionForce; }
380 set { _wheelsSuspensionForce = value; }
381 }
382
383 public Single SkidInfo
384 {
385 get { return _skidInfo; }
386 set { _skidInfo = value; }
387 }
388 #endregion
389
390 /// <summary>
391 ///
392 /// </summary>
393 /// <param name="chassis"></param>
394 /// <param name="paramRaycastInfo">Not used!</param>
395 public void UpdateWheel(RigidBody chassis, RaycastInfo paramRaycastInfo)
396 {
397 if (_raycastInfo.IsInContact)
398 {
399 float project = Vector3.Dot(_raycastInfo.ContactNormalWS, _raycastInfo.WheelDirectionWS);
400
401 Vector3 chassisVelocityAtContactPoint = new Vector3();
402 Vector3 relpos = _raycastInfo.ContactPointWS - chassis.CenterOfMassPosition;
403 chassisVelocityAtContactPoint = chassis.GetVelocityInLocalPoint(relpos);
404 float projVel = Vector3.Dot(_raycastInfo.ContactNormalWS, chassisVelocityAtContactPoint);
405
406 if (project >= -0.1f)
407 {
408 _suspensionRelativeVelocity = 0;
409 _clippedInvContactDotSuspension = 1.0f / 0.1f;
410 }
411 else
412 {
413 float inv = -1 / project;
414 _suspensionRelativeVelocity = projVel * inv;
415 _clippedInvContactDotSuspension = inv;
416 }
417 }
418 else
419 {
420 _raycastInfo.SuspensionLength = _suspensionRestLength;
421 _suspensionRelativeVelocity = 0.0f;
422 _raycastInfo.ContactNormalWS = -_raycastInfo.WheelDirectionWS;
423 _clippedInvContactDotSuspension = 1.0f;
424 }
425 }
426
427 // if (m_raycastInfo.m_isInContact)
428
429 //{
430 // btScalar project= m_raycastInfo.m_contactNormalWS.dot( m_raycastInfo.m_wheelDirectionWS );
431 // btVector3 chassis_velocity_at_contactPoint;
432 // btVector3 relpos = m_raycastInfo.m_contactPointWS - chassis.getCenterOfMassPosition();
433 // chassis_velocity_at_contactPoint = chassis.getVelocityInLocalPoint( relpos );
434 // btScalar projVel = m_raycastInfo.m_contactNormalWS.dot( chassis_velocity_at_contactPoint );
435 // if ( project >= -0.1f)
436 // {
437 // m_suspensionRelativeVelocity = 0.0f;
438 // m_clippedInvContactDotSuspension = 1.0f / 0.1f;
439 // }
440 // else
441 // {
442 // btScalar inv = -1.f / project;
443 // m_suspensionRelativeVelocity = projVel * inv;
444 // m_clippedInvContactDotSuspension = inv;
445 // }
446
447 //}
448
449 //else // Not in contact : position wheel in a nice (rest length) position
450 //{
451 // m_raycastInfo.m_suspensionLength = this->getSuspensionRestLength();
452 // m_suspensionRelativeVelocity = 0.0f;
453 // m_raycastInfo.m_contactNormalWS = -m_raycastInfo.m_wheelDirectionWS;
454 // m_clippedInvContactDotSuspension = 1.0f;
455 //}
456 };
457
458 //btScalar m_clippedInvContactDotSuspension;
459 //btScalar m_suspensionRelativeVelocity;
460 //btScalar m_wheelsSuspensionForce;
461 //btScalar m_skidInfo;
462
463 //void* m_clientInfo;//can be used to store pointer to sync transforms...
464
465 //btWheelInfo(btWheelInfoConstructionInfo& ci)
466
467 //{
468
469 // m_suspensionRestLength1 = ci.m_suspensionRestLength;
470 // m_maxSuspensionTravelCm = ci.m_maxSuspensionTravelCm;
471
472 // m_wheelsRadius = ci.m_wheelRadius;
473 // m_suspensionStiffness = ci.m_suspensionStiffness;
474 // m_wheelsDampingCompression = ci.m_wheelsDampingCompression;
475 // m_wheelsDampingRelaxation = ci.m_wheelsDampingRelaxation;
476 // m_chassisConnectionPointCS = ci.m_chassisConnectionCS;
477 // m_wheelDirectionCS = ci.m_wheelDirectionCS;
478 // m_wheelAxleCS = ci.m_wheelAxleCS;
479 // m_frictionSlip = ci.m_frictionSlip;
480 // m_steering = 0.f;
481 // m_engineForce = 0.f;
482 // m_rotation = 0.f;
483 // m_deltaRotation = 0.f;
484 // m_brake = 0.f;
485 // m_rollInfluence = 0.1f;
486 // m_bIsFrontWheel = ci.m_bIsFrontWheel;
487
488 //}
489
490 //void updateWheel(const btRigidBody& chassis,RaycastInfo& raycastInfo);
491
492 //btScalar m_clippedInvContactDotSuspension;
493 //btScalar m_suspensionRelativeVelocity;
494 ////calculated by suspension
495 //btScalar m_wheelsSuspensionForce;
496 //btScalar m_skidInfo;
497
498 //};
499
500 //struct RaycastInfo
501 //{
502 // //set by raycaster
503 // btVector3 m_contactNormalWS;//contactnormal
504 // btVector3 m_contactPointWS;//raycast hitpoint
505 // btScalar m_suspensionLength;
506 // btVector3 m_hardPointWS;//raycast starting point
507 // btVector3 m_wheelDirectionWS; //direction in worldspace
508 // btVector3 m_wheelAxleWS; // axle in worldspace
509 // bool m_isInContact;
510 // void* m_groundObject; //could be general void* ptr
511 //};
512
513 //struct btWheelInfoConstructionInfo
514 //{
515 // btVector3 m_chassisConnectionCS;
516 // btVector3 m_wheelDirectionCS;
517 // btVector3 m_wheelAxleCS;
518 // btScalar m_suspensionRestLength;
519 // btScalar m_maxSuspensionTravelCm;
520 // btScalar m_wheelRadius;
521
522 // float m_suspensionStiffness;
523 // float m_wheelsDampingCompression;
524 // float m_wheelsDampingRelaxation;
525 // float m_frictionSlip;
526 // bool m_bIsFrontWheel;
527
528 //};
529}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs
new file mode 100644
index 0000000..abd4885
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Exceptions/BulletException.cs
@@ -0,0 +1,53 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using System.Runtime.Serialization;
26
27namespace XnaDevRu.BulletX
28{
29 public class BulletException : Exception
30 {
31 internal BulletException()
32 : base("Bullet Physics Library has thrown an exception.")
33 {
34 }
35
36 internal BulletException(string message)
37 : base(message)
38 {
39 }
40
41 internal BulletException(string message, Exception innerException)
42 : base(message, innerException)
43 {
44 }
45
46#if !XBOX
47 internal BulletException(SerializationInfo info, StreamingContext context)
48 : base(info, context)
49 {
50 }
51#endif
52 }
53}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs b/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs
new file mode 100644
index 0000000..ef5b5dc
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/IDebugDraw.cs
@@ -0,0 +1,60 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public enum DebugDrawModes
30 {
31 NoDebug = 0,
32 DrawWireframe = 1,
33 DrawAabb = 2,
34 DrawFeaturesText = 4,
35 DrawContactPoints = 8,
36 NoDeactivation = 16,
37 NoHelpText = 32,
38 DrawText = 64,
39 ProfileTimings = 128,
40 EnableSatComparison = 256,
41 DisableBulletLcp = 512,
42 EnableCcd = 1024,
43 MaxDebugDrawMode
44 }
45
46 public interface IDebugDraw
47 {
48 void DrawLine(Vector3 from, Vector3 to, Vector3 color);
49
50 void DrawContactPoint(
51 Vector3 pointOnB,
52 Vector3 normalOnB,
53 float distance,
54 int lifeTime,
55 Vector3 color
56 );
57
58 DebugDrawModes DebugMode { get; set; }
59 }
60} \ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs
new file mode 100644
index 0000000..2a45a20
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/DefaultMotionState.cs
@@ -0,0 +1,66 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 // DefaultMotionState provides a common implementation to synchronize world transforms with offsets
30 public class DefaultMotionState : MotionState
31 {
32 private Matrix _graphicsWorldTransform;
33 private Matrix _centerOfMassOffset;
34 private Matrix _startWorldTransform;
35 private object _userData;
36
37 public DefaultMotionState()
38 : this(Matrix.Identity, Matrix.Identity) { }
39
40 public DefaultMotionState(Matrix startTransform, Matrix centerOfMassOffset)
41 {
42 _graphicsWorldTransform = startTransform;
43 _centerOfMassOffset = centerOfMassOffset;
44 _startWorldTransform = startTransform;
45 }
46
47 public Matrix GraphicsWorldTransform { get { return _graphicsWorldTransform; } set { _graphicsWorldTransform = value; } }
48 public Matrix CenterOfMassOffset { get { return _centerOfMassOffset; } set { _centerOfMassOffset = value; } }
49 public Matrix StartWorldTransform { get { return _startWorldTransform; } set { _startWorldTransform = value; } }
50 public object UserData { get { return _userData; } set { _userData = value; } }
51
52 // synchronizes world transform from user to physics
53 public override void GetWorldTransform(out Matrix centerOfMassWorldTrans)
54 {
55 centerOfMassWorldTrans = MathHelper.InvertMatrix(_centerOfMassOffset) * _graphicsWorldTransform;
56 }
57
58 // synchronizes world transform from physics to user
59 // Bullet only calls the update of worldtransform for active objects
60 public override void SetWorldTransform(Matrix centerOfMassWorldTrans)
61 {
62 _graphicsWorldTransform = MatrixOperations.Multiply(centerOfMassWorldTrans, _centerOfMassOffset);
63 _graphicsWorldTransform.Translation = centerOfMassWorldTrans.Translation;
64 }
65 }
66}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs
new file mode 100644
index 0000000..2e3f258
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MathHelper.cs
@@ -0,0 +1,581 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26using MonoXnaCompactMaths;
27
28namespace XnaDevRu.BulletX
29{
30 public static class MathHelper
31 {
32 internal const float Sqrt12 = 0.7071067811865475244008443621048490f;
33 internal const float Infinity = 3.402823466e+38f;
34 internal const float Epsilon = 1.192092896e-07f;
35
36 public static Vector3 MatrixToVector(Matrix m, Vector3 v)
37 {
38 return new Vector3(
39 Vector3.Dot(new Vector3(m.M11, m.M12, m.M13), v) + m.Translation.X,
40 Vector3.Dot(new Vector3(m.M21, m.M22, m.M23), v) + m.Translation.Y,
41 Vector3.Dot(new Vector3(m.M31, m.M32, m.M33), v) + m.Translation.Z
42 );
43 }
44
45 internal static int ClosestAxis(Vector4 v)
46 {
47 return MaxAxis(Absolute(v));
48 }
49
50 internal static Vector4 Absolute(Vector4 v)
51 {
52 return new Vector4(Math.Abs(v.X), Math.Abs(v.Y), Math.Abs(v.Z), Math.Abs(v.W));
53 }
54
55 internal static int MaxAxis(Vector4 v)
56 {
57 int maxIndex = -1;
58 float maxVal = float.MinValue;
59 if (v.X > maxVal)
60 {
61 maxIndex = 0;
62 maxVal = v.X;
63 }
64 if (v.Y > maxVal)
65 {
66 maxIndex = 1;
67 maxVal = v.Y;
68 }
69 if (v.Z > maxVal)
70 {
71 maxIndex = 2;
72 maxVal = v.Z;
73 }
74 if (v.W > maxVal)
75 {
76 maxIndex = 3;
77 maxVal = v.W;
78 }
79
80 return maxIndex;
81 }
82
83 internal static int MaxAxis(Vector3 v)
84 {
85 return v.X < v.Y ? (v.Y < v.Z ? 2 : 1) : (v.X < v.Z ? 2 : 0);
86 }
87
88 // conservative test for overlap between two aabbs
89 internal static bool TestAabbAgainstAabb2(Vector3 aabbMinA, Vector3 aabbMaxA, Vector3 aabbMinB, Vector3 aabbMaxB)
90 {
91 bool overlap = true;
92 overlap = (aabbMinA.X > aabbMaxB.X || aabbMaxA.X < aabbMinB.X) ? false : overlap;
93 overlap = (aabbMinA.Z > aabbMaxB.Z || aabbMaxA.Z < aabbMinB.Z) ? false : overlap;
94 overlap = (aabbMinA.Y > aabbMaxB.Y || aabbMaxA.Y < aabbMinB.Y) ? false : overlap;
95 return overlap;
96 }
97
98 internal static bool TestTriangleAgainstAabb2(Vector3[] vertices, Vector3 aabbMin, Vector3 aabbMax)
99 {
100 Vector3 p1 = vertices[0];
101 Vector3 p2 = vertices[1];
102 Vector3 p3 = vertices[2];
103
104 if (Math.Min(Math.Min(p1.X, p2.X), p3.X) > aabbMax.X) return false;
105 if (Math.Max(Math.Max(p1.X, p2.X), p3.X) < aabbMin.X) return false;
106
107 if (Math.Min(Math.Min(p1.Z, p2.Z), p3.Z) > aabbMax.Z) return false;
108 if (Math.Max(Math.Max(p1.Z, p2.Z), p3.Z) < aabbMin.Z) return false;
109
110 if (Math.Min(Math.Min(p1.Y, p2.Y), p3.Y) > aabbMax.Y) return false;
111 if (Math.Max(Math.Max(p1.Y, p2.Y), p3.Y) < aabbMin.Y) return false;
112 return true;
113 }
114
115 internal static void SetInterpolate3(Vector3 vA, Vector3 vB, float rt, ref Vector3 interpolated)
116 {
117 float s = 1.0f - rt;
118 interpolated.X = s * vA.X + rt * vB.X;
119 interpolated.Y = s * vA.Y + rt * vB.Y;
120 interpolated.Z = s * vA.Z + rt * vB.Z;
121 }
122
123 internal static void PlaneSpace1(Vector3 n, ref Vector3 p, ref Vector3 q)
124 {
125 if (Math.Abs(n.Z) > Sqrt12)
126 {
127 // choose p in y-z plane
128 float a = n.Y * n.Y + n.Z * n.Z;
129 float k = 1f / (float)Math.Sqrt(a);
130 p.X = 0;
131 p.Y = -n.Z * k;
132 p.Z = n.Y * k;
133 // set q = n x p
134 q.X = a * k;
135 q.Y = -n.X * p.Z;
136 q.Z = n.X * p.Y;
137 }
138 else
139 {
140 // choose p in x-y plane
141 float a = n.X * n.X + n.Y * n.Y;
142 float k = 1f / (float)Math.Sqrt(a);
143 p.X = -n.Y * k;
144 p.Y = n.X * k;
145 p.Z = 0;
146 // set q = n x p
147 q.X = -n.Z * p.Y;
148 q.Y = n.Z * p.X;
149 q.Z = a * k;
150 }
151 }
152
153 internal static bool RayAabb(Vector3 rayFrom,
154 Vector3 rayTo,
155 Vector3 aabbMin,
156 Vector3 aabbMax,
157 float param, Vector3 normal)
158 {
159 Vector3 aabbHalfExtent = (aabbMax - aabbMin) * 0.5f;
160 Vector3 aabbCenter = (aabbMax + aabbMin) * 0.5f;
161 Vector3 source = rayFrom - aabbCenter;
162 Vector3 target = rayTo - aabbCenter;
163 int sourceOutcode = Outcode(source, aabbHalfExtent);
164 int targetOutcode = Outcode(target, aabbHalfExtent);
165 if ((sourceOutcode & targetOutcode) == 0x0)
166 {
167 float lambda_enter = 0;
168 float lambda_exit = param;
169 Vector3 r = target - source;
170 float normSign = 1;
171 Vector3 hitNormal = new Vector3();
172 int bit = 1;
173
174 for (int j = 0; j < 2; j++)
175 {
176 {
177 if ((sourceOutcode & bit) != 0)
178 {
179 float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X;
180 if (lambda_enter <= lambda)
181 {
182 lambda_enter = lambda;
183 hitNormal = new Vector3();
184 hitNormal.X = normSign;
185 }
186 }
187 else if ((targetOutcode & bit) != 0)
188 {
189 float lambda = (-source.X - aabbHalfExtent.X * normSign) / r.X;
190 SetMin(ref lambda_exit, lambda);
191 }
192 bit <<= 1;
193 }
194 {
195 if ((sourceOutcode & bit) != 0)
196 {
197 float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y;
198 if (lambda_enter <= lambda)
199 {
200 lambda_enter = lambda;
201 hitNormal = new Vector3();
202 hitNormal.Y = normSign;
203 }
204 }
205 else if ((targetOutcode & bit) != 0)
206 {
207 float lambda = (-source.Y - aabbHalfExtent.Y * normSign) / r.Y;
208 SetMin(ref lambda_exit, lambda);
209 }
210 bit <<= 1;
211 }
212 {
213 if ((sourceOutcode & bit) != 0)
214 {
215 float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z;
216 if (lambda_enter <= lambda)
217 {
218 lambda_enter = lambda;
219 hitNormal = new Vector3();
220 hitNormal.Z = normSign;
221 }
222 }
223 else if ((targetOutcode & bit) != 0)
224 {
225 float lambda = (-source.Z - aabbHalfExtent.Z * normSign) / r.Z;
226 SetMin(ref lambda_exit, lambda);
227 }
228 bit <<= 1;
229 }
230 normSign = -1;
231 }
232 if (lambda_enter <= lambda_exit)
233 {
234 param = lambda_enter;
235 normal = hitNormal;
236 return true;
237 }
238 }
239 return false;
240 }
241
242 internal static void SetMin(ref float a, float b)
243 {
244 if (a > b)
245 a = b;
246 }
247
248 internal static void SetMax(ref float a, float b)
249 {
250 if (a < b)
251 a = b;
252 }
253
254 internal static void SetMax(ref Vector3 self, Vector3 other)
255 {
256 if (other.X > self.X)
257 self.X = other.X;
258
259 if (other.Y > self.Y)
260 self.Y = other.Y;
261
262 if (other.Z > self.Z)
263 self.Z = other.Z;
264 }
265
266 internal static Vector3 SetMax(Vector3 self, Vector3 other)
267 {
268 if (other.X > self.X)
269 self.X = other.X;
270
271 if (other.Y > self.Y)
272 self.Y = other.Y;
273
274 if (other.Z > self.Z)
275 self.Z = other.Z;
276
277 return self;
278 }
279
280 internal static void SetMin(ref Vector3 self, Vector3 other)
281 {
282 if (other.X < self.X)
283 self.X = other.X;
284
285 if (other.Y < self.Y)
286 self.Y = other.Y;
287
288 if (other.Z < self.Z)
289 self.Z = other.Z;
290 }
291
292 internal static Vector3 SetMin(Vector3 self, Vector3 other)
293 {
294 if (other.X < self.X)
295 self.X = other.X;
296
297 if (other.Y < self.Y)
298 self.Y = other.Y;
299
300 if (other.Z < self.Z)
301 self.Z = other.Z;
302
303 return self;
304 }
305
306 internal static int Outcode(Vector3 p, Vector3 halfExtent)
307 {
308 return (p.X < -halfExtent.X ? 0x01 : 0x0) |
309 (p.X > halfExtent.X ? 0x08 : 0x0) |
310 (p.Y < -halfExtent.Y ? 0x02 : 0x0) |
311 (p.Y > halfExtent.Y ? 0x10 : 0x0) |
312 (p.Z < -halfExtent.Z ? 0x4 : 0x0) |
313 (p.Z > halfExtent.Z ? 0x20 : 0x0);
314 }
315
316 internal static Matrix Absolute(Matrix m)
317 {
318 return new Matrix(Math.Abs(m.M11), Math.Abs(m.M12), Math.Abs(m.M13), Math.Abs(m.M14),
319 Math.Abs(m.M21), Math.Abs(m.M22), Math.Abs(m.M23), Math.Abs(m.M24),
320 Math.Abs(m.M31), Math.Abs(m.M32), Math.Abs(m.M33), Math.Abs(m.M34),
321 Math.Abs(m.M41), Math.Abs(m.M42), Math.Abs(m.M43), Math.Abs(m.M44));
322 }
323
324 internal static void SetValueByIndex(ref Vector3 v, int i, float value)
325 {
326 if (i == 0)
327 v.X = value;
328 else if (i == 1)
329 v.Y = value;
330 else
331 v.Z = value;
332 }
333
334 internal static float GetValueByIndex(Vector3 v, int i)
335 {
336 if (i == 0)
337 return v.X;
338 else if (i == 1)
339 return v.Y;
340 else
341 return v.Z;
342 }
343
344 internal static Vector3 InvXForm(Matrix m, Vector3 v)
345 {
346 v -= m.Translation;
347 m.Translation = new Vector3();
348 return MathHelper.Transform(v, Matrix.Transpose(m));
349 }
350
351 internal static Matrix InverseTimes(Matrix m, Matrix t)
352 {
353 Vector3 v = t.Translation - m.Translation;
354
355 Matrix mat = TransposeTimes(m, t);
356 mat.Translation = Vector3.Transform(v, m);
357 return mat;
358 }
359
360 internal static Matrix TransposeTimes(Matrix mA, Matrix mB)
361 {
362 return new Matrix(
363 mA.M11 * mB.M11 + mA.M21 * mB.M21 + mA.M31 * mB.M31,
364 mA.M11 * mB.M12 + mA.M21 * mB.M22 + mA.M31 * mB.M32,
365 mA.M11 * mB.M13 + mA.M21 * mB.M23 + mA.M31 * mB.M33,
366 0,
367 mA.M12 * mB.M11 + mA.M22 * mB.M21 + mA.M32 * mB.M31,
368 mA.M12 * mB.M12 + mA.M22 * mB.M22 + mA.M32 * mB.M32,
369 mA.M12 * mB.M13 + mA.M22 * mB.M23 + mA.M32 * mB.M33,
370 0,
371 mA.M13 * mB.M11 + mA.M23 * mB.M21 + mA.M33 * mB.M31,
372 mA.M13 * mB.M12 + mA.M23 * mB.M22 + mA.M33 * mB.M32,
373 mA.M13 * mB.M13 + mA.M23 * mB.M23 + mA.M33 * mB.M33,
374 0, 0, 0, 0, 1);
375 }
376
377 internal static Vector3 GetColumn(Matrix m, int column)
378 {
379 switch (column)
380 {
381 case 1:
382 return new Vector3(m.M11, m.M21, m.M31);
383 case 2:
384 return new Vector3(m.M12, m.M22, m.M32);
385 case 3:
386 return new Vector3(m.M13, m.M23, m.M33);
387 default:
388 throw new ArgumentOutOfRangeException("column");
389 }
390 }
391
392 internal static Vector3 GetRow(Matrix m, int row)
393 {
394 switch (row)
395 {
396 case 1:
397 return new Vector3(m.M11, m.M12, m.M13);
398 case 2:
399 return new Vector3(m.M21, m.M22, m.M23);
400 case 3:
401 return new Vector3(m.M31, m.M32, m.M33);
402 default:
403 throw new ArgumentOutOfRangeException("row");
404 }
405 }
406
407 internal static Quaternion GetRotation(Matrix m)
408 {
409 float trace = m.M11 + m.M22 + m.M33;
410 Quaternion q = new Quaternion();
411
412 if (trace > 0)
413 {
414 float s = (float)Math.Sqrt(trace + 1.0f);
415 q.W = s * 0.5f;
416 s = 0.5f / s;
417
418 q.X = (m.M32 - m.M23) * s;
419 q.Y = (m.M13 - m.M31) * s;
420 q.Z = (m.M21 - m.M12) * s;
421 }
422 else
423 {
424 int i = m.M11 < m.M22 ?
425 (m.M22 < m.M33 ? 2 : 1) :
426 (m.M11 < m.M33 ? 2 : 0);
427 int j = (i + 1) % 3;
428 int k = (i + 2) % 3;
429
430 float s = (float)Math.Sqrt(GetElement(m, i, i) - GetElement(m, j, j) - GetElement(m, k, k) + 1.0f);
431 SetElement(ref q, i, s * 0.5f);
432 s = 0.5f / s;
433
434 q.W = (GetElement(m, k, j) - GetElement(m, j, k)) * s;
435 SetElement(ref q, j, (GetElement(m, j, i) + GetElement(m, i, j)) * s);
436 SetElement(ref q, k, (GetElement(m, k, i) + GetElement(m, i, k)) * s);
437 }
438 return q;
439 }
440
441 internal static float SetElement(ref Quaternion q, int index, float value)
442 {
443 switch (index)
444 {
445 case 0:
446 q.X = value; break;
447 case 1:
448 q.Y = value; break;
449 case 2:
450 q.Z = value; break;
451 case 3:
452 q.W = value; break;
453 }
454
455 return 0;
456 }
457
458 internal static float GetElement(Quaternion q, int index)
459 {
460 switch (index)
461 {
462 case 0:
463 return q.X;
464 case 1:
465 return q.Y;
466 case 2:
467 return q.Z;
468 default:
469 return q.W;
470 }
471 }
472
473 internal static float GetElement(Matrix mat, int index)
474 {
475 int row = index % 3;
476 int col = index / 3;
477
478 return GetElement(mat, row, col);
479 }
480
481 internal static float GetElement(Matrix mat, int row, int col)
482 {
483 switch (row)
484 {
485 case 0:
486 switch (col)
487 {
488 case 0:
489 return mat.M11;
490 case 1:
491 return mat.M12;
492 case 2:
493 return mat.M13;
494 } break;
495 case 1:
496 switch (col)
497 {
498 case 0:
499 return mat.M21;
500 case 1:
501 return mat.M22;
502 case 2:
503 return mat.M23;
504 } break;
505 case 2:
506 switch (col)
507 {
508 case 0:
509 return mat.M31;
510 case 1:
511 return mat.M32;
512 case 2:
513 return mat.M33;
514 } break;
515 }
516
517 return 0;
518 }
519
520 internal static float GetElement(Vector3 v, int index)
521 {
522 if (index == 0)
523 return v.X;
524 if (index == 1)
525 return v.Y;
526 if (index == 2)
527 return v.Z;
528
529 throw new ArgumentOutOfRangeException("index");
530 }
531
532 internal static void SetElement(ref Vector3 v, int index, float value)
533 {
534 if (index == 0)
535 v.X = value;
536 else if (index == 1)
537 v.Y = value;
538 else if (index == 2)
539 v.Z = value;
540 else
541 throw new ArgumentOutOfRangeException("index");
542 }
543
544 public static Matrix InvertMatrix(Matrix m)
545 {
546 Vector3 pos = m.Translation;
547 m.Translation = Vector3.Zero;
548 Matrix inv = Matrix.Transpose(m);
549 pos = Vector3.Transform(-pos, m);
550 inv.Translation = pos;
551 return inv;
552 }
553
554 public static Matrix GetDisplayMatrix(Matrix m)
555 {
556 Matrix displayMatrix = m;
557 displayMatrix.Translation = Vector3.Zero;
558 displayMatrix = Matrix.Transpose(displayMatrix);
559 displayMatrix.Translation = m.Translation;
560 return displayMatrix;
561 }
562
563 internal static Vector3 Transform(Vector3 position, Matrix matrix)
564 {
565 Vector3 vector = new Vector3();
566 vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13)) + matrix.M41;
567 vector.Y = (((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23)) + matrix.M42;
568 vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33)) + matrix.M43;
569 return vector;
570 }
571
572 internal static Vector3 TransformNormal(Vector3 position, Matrix matrix)
573 {
574 Vector3 vector = new Vector3();
575 vector.X = (((position.X * matrix.M11) + (position.Y * matrix.M12)) + (position.Z * matrix.M13));
576 vector.Y = (((position.X * matrix.M21) + (position.Y * matrix.M22)) + (position.Z * matrix.M23));
577 vector.Z = (((position.X * matrix.M31) + (position.Y * matrix.M32)) + (position.Z * matrix.M33));
578 return vector;
579 }
580 }
581}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs
new file mode 100644
index 0000000..04e42d9
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MatrixOperations.cs
@@ -0,0 +1,116 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 internal static class MatrixOperations
30 {
31 public static void SetRotation(ref Matrix m, Quaternion q)
32 {
33 float d = q.LengthSquared();
34 BulletDebug.Assert(d != 0);
35 float s = 2f / d;
36 float xs = q.X * s, ys = q.Y * s, zs = q.Z * s;
37 float wx = q.W * xs, wy = q.W * ys, wz = q.W * zs;
38 float xx = q.X * xs, xy = q.X * ys, xz = q.X * zs;
39 float yy = q.Y * ys, yz = q.Y * zs, zz = q.Z * zs;
40 m = new Matrix(1 - (yy + zz), xy - wz, xz + wy, 0,
41 xy + wz, 1 - (xx + zz), yz - wx, 0,
42 xz - wy, yz + wx, 1 - (xx + yy), 0,
43 m.M41, m.M42, m.M43, 1);
44 }
45
46 public static Quaternion GetRotation(Matrix m)
47 {
48 Quaternion q = new Quaternion();
49
50 float trace = m.M11 + m.M22 + m.M33;
51
52 if (trace > 0)
53 {
54 float s = (float)Math.Sqrt(trace + 1);
55 q.W = s * 0.5f;
56 s = 0.5f / s;
57
58 q.X = (m.M32 - m.M23) * s;
59 q.Y = (m.M13 - m.M31) * s;
60 q.Z = (m.M21 - m.M12) * s;
61 }
62 else
63 {
64 int i = m.M11 < m.M22 ?
65 (m.M22 < m.M33 ? 2 : 1) :
66 (m.M11 < m.M33 ? 2 : 0);
67 int j = (i + 1) % 3;
68 int k = (i + 2) % 3;
69
70 float s = (float)Math.Sqrt(MathHelper.GetElement(m, i, i) - MathHelper.GetElement(m, j, j) - MathHelper.GetElement(m, k, k) + 1);
71 MathHelper.SetElement(ref q, i, s * 0.5f);
72 s = 0.5f / s;
73
74 q.W = (MathHelper.GetElement(m, k, j) - MathHelper.GetElement(m, j, k)) * s;
75 MathHelper.SetElement(ref q, j, (MathHelper.GetElement(m, j, i) + MathHelper.GetElement(m, i, j)) * s);
76 MathHelper.SetElement(ref q, k, (MathHelper.GetElement(m, k, i) + MathHelper.GetElement(m, i, k)) * s);
77 }
78
79 return q;
80 }
81
82 public static Matrix Scaled(Matrix m, Vector3 v)
83 {
84 return new Matrix( m.M11 * v.X, m.M12 * v.Y, m.M13 * v.Z, 0,
85 m.M21 * v.X, m.M22 * v.Y, m.M23 * v.Z, 0,
86 m.M31 * v.X, m.M32 * v.Y, m.M33 * v.Z, 0,
87 0, 0, 0, 1);
88 }
89
90 public static Matrix Multiply(Matrix a, Matrix b)
91 {
92 /*return btMatrix3x3(
93 m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]),
94 m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]),
95 m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2]));*/
96 return new Matrix(
97 Dot(b, 0, MathHelper.GetRow(a, 1)), Dot(b, 1, MathHelper.GetRow(a, 1)), Dot(b, 2, MathHelper.GetRow(a, 1)), 0,
98 Dot(b, 0, MathHelper.GetRow(a, 2)), Dot(b, 1, MathHelper.GetRow(a, 2)), Dot(b, 2, MathHelper.GetRow(a, 2)), 0,
99 Dot(b, 0, MathHelper.GetRow(a, 3)), Dot(b, 1, MathHelper.GetRow(a, 3)), Dot(b, 2, MathHelper.GetRow(a, 3)), 0,
100 0, 0, 0, 1);
101 }
102
103 public static float Dot(Matrix m, int c, Vector3 v)
104 {
105 return MathHelper.GetElement(m, 0, c) * v.X + MathHelper.GetElement(m, 1, c) * v.Y + MathHelper.GetElement(m, 2, c) * v.Z;
106 }
107
108 public static Matrix Transpose(Matrix m)
109 {
110 return new Matrix( m.M11, m.M21, m.M31, 0,
111 m.M12, m.M22, m.M32, 0,
112 m.M13, m.M23, m.M33, 0,
113 0, 0, 0, 1);
114 }
115 }
116}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs
new file mode 100644
index 0000000..4039177
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/MotionState.cs
@@ -0,0 +1,44 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public abstract class MotionState
30 {
31 /*public abstract Vector3 getWorldPosition();
32 public abstract Vector3 getWorldScaling();
33 public abstract Quaternion getWorldOrientation();
34
35 public abstract void setWorldPosition(Vector3 worldPos);
36 public abstract void setWorldOrientation(Quaternion orn);
37
38 public abstract void calculateWorldTransformations();*/
39
40 public abstract void GetWorldTransform(out Matrix worldTransform);
41 //Bullet only calls the update of worldtransform for active objects
42 public abstract void SetWorldTransform(Matrix worldTransform);
43 }
44}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs
new file mode 100644
index 0000000..35c9fed
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/QuadWord.cs
@@ -0,0 +1,88 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26namespace XnaDevRu.BulletX.LinearMath
27{
28 internal abstract class QuadWord
29 {
30 private float x;
31 private float y;
32 private float z;
33 private float w;
34
35 public float X { get { return x; } set { x = value; } }
36 public float Y { get { return y; } set { y = value; } }
37 public float Z { get { return z; } set { z = value; } }
38 public float W { get { return w; } set { w = value; } }
39
40 public QuadWord() { }
41
42 public QuadWord(float x, float y, float z, float w)
43 {
44 X = x;
45 Y = y;
46 Z = z;
47 W = w;
48 }
49
50 public QuadWord(float x, float y, float z)
51 {
52 X = x;
53 Y = y;
54 Z = z;
55 W = 0;
56 }
57
58 public void SetMax(QuadWord other)
59 {
60 if (other.X > X)
61 X = other.X;
62
63 if (other.Y > Y)
64 Y = other.Y;
65
66 if (other.Z > Z)
67 Z = other.Z;
68
69 if (other.W > W)
70 W = other.W;
71 }
72
73 public void SetMin(QuadWord other)
74 {
75 if (other.X < X)
76 X = other.X;
77
78 if (other.Y < Y)
79 Y = other.Y;
80
81 if (other.Z < Z)
82 Z = other.Z;
83
84 if (other.W < W)
85 W = other.W;
86 }
87 }
88}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs
new file mode 100644
index 0000000..5425a05
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Quaternion.cs
@@ -0,0 +1,198 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26namespace XnaDevRu.BulletX.LinearMath
27{
28 internal class Quaternion : QuadWord
29 {
30 public Quaternion() { }
31
32 public Quaternion(float x, float y, float z, float w)
33 : base(x, y, z, w) { }
34
35 public Quaternion(Vector3 axis, float angle)
36 {
37 SetRotation(axis, angle);
38 }
39
40 public Quaternion(float yaw, float pitch, float roll)
41 {
42 SetEuler(yaw, pitch, roll);
43 }
44
45 public void SetRotation(Vector3 axis, float angle)
46 {
47 float d = axis.Length();
48 if (d == 0) throw new DivideByZeroException();
49 float s = (float)Math.Sin(angle * 0.5f) / d;
50 X = axis.X * s;
51 Y = axis.Y * s;
52 Z = axis.Z * s;
53 W = (float)Math.Cos(angle * 0.5f);
54 }
55
56 public void SetEuler(float yaw, float pitch, float roll)
57 {
58 float halfYaw = yaw * 0.5f;
59 float halfPitch = pitch * 0.5f;
60 float halfRoll = roll * 0.5f;
61 float cosYaw = (float)Math.Cos(halfYaw);
62 float sinYaw = (float)Math.Sin(halfYaw);
63 float cosPitch = (float)Math.Cos(halfPitch);
64 float sinPitch = (float)Math.Sin(halfPitch);
65 float cosRoll = (float)Math.Cos(halfRoll);
66 float sinRoll = (float)Math.Sin(halfRoll);
67 X = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw;
68 Y = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw;
69 Z = sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw;
70 W = cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw;
71 }
72
73 public float LengthSquared()
74 {
75 return Dot(this, this);
76 }
77
78 public float Length()
79 {
80 return (float)Math.Sqrt(LengthSquared());
81 }
82
83 public float Angle()
84 {
85 return 2f * (float)Math.Acos(W);
86 }
87
88 public static float Angle(Quaternion a, Quaternion b)
89 {
90 float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared());
91 if (s == 0) throw new DivideByZeroException();
92 return (float)Math.Acos(Dot(a, b) / s);
93 }
94
95 public static Quaternion Farthest(Quaternion a, Quaternion b)
96 {
97 Quaternion diff, sum;
98 diff = a - b;
99 sum = a + b;
100 if (Dot(diff, diff) > Dot(sum, sum))
101 return b;
102 return -b;
103 }
104
105 public static Quaternion Slerp(Quaternion a, Quaternion b, float c)
106 {
107 float theta = Angle(a, b);
108 if (theta != 0)
109 {
110 float d = 1f / (float)Math.Sin(theta);
111 float s0 = (float)Math.Sin((1f - c) * theta);
112 float s1 = (float)Math.Sin(c * theta);
113 return new Quaternion(
114 (a.X * s0 + b.X * s1) * d,
115 (a.Y * s0 + b.Y * s1) * d,
116 (a.Z * s0 + b.Z * s1) * d,
117 (a.W * s0 + b.W * s1) * d);
118 }
119 else
120 {
121 return a;
122 }
123 }
124
125 public static Quaternion Inverse(Quaternion a)
126 {
127 return new Quaternion(a.X, a.Y, a.Z, -a.W);
128 }
129
130 public static Quaternion Normalize(Quaternion a)
131 {
132 return a / a.Length();
133 }
134
135 public static float Dot(Quaternion a, Quaternion b)
136 {
137 return a.X * b.X + a.Y * b.Y + a.Z * b.Z + a.W * b.W;
138 }
139
140 public static Quaternion operator +(Quaternion a, Quaternion b)
141 {
142 return new Quaternion(a.X + b.X, a.Y + b.Y, a.Z + b.Z, a.W + b.W);
143 }
144
145 public static Quaternion operator -(Quaternion a, Quaternion b)
146 {
147 return new Quaternion(a.X - b.X, a.Y - b.Y, a.Z - b.Z, a.W - b.W);
148 }
149
150 public static Quaternion operator -(Quaternion a)
151 {
152 return new Quaternion(-a.X, -a.Y, -a.Z, -a.W);
153 }
154
155 public static Quaternion operator *(Quaternion a, float b)
156 {
157 return new Quaternion(a.X * b, a.Y * b, a.Z * b, a.W * b);
158 }
159
160 public static Quaternion operator *(Quaternion a, Quaternion b)
161 {
162 return new Quaternion(
163 a.W * b.X + a.X * b.W + a.Y * b.Z - a.Z * b.Y,
164 a.W * b.Y + a.Y * b.W + a.Z * b.X - a.X * b.Z,
165 a.W * b.Z + a.Z * b.W + a.X * b.Y - a.Y * b.X,
166 a.W * b.W - a.X * b.X - a.Y * b.Y - a.Z * b.Z);
167 }
168
169 public static Quaternion operator *(Quaternion a, Vector3 b)
170 {
171 return new Quaternion(
172 a.W * b.X + a.Y * b.Z - a.Z * b.Y,
173 a.W * b.Y + a.Z * b.X - a.X * b.Z,
174 a.W * b.Z + a.X * b.Y - a.Y * b.X,
175 -a.X * b.X - a.Y * b.Y - a.Z * b.Z);
176 }
177
178 public static Quaternion operator *(Vector3 w, Quaternion q)
179 {
180 return new Quaternion(
181 w.X * q.W + w.Y * q.Z - w.Z * q.Y,
182 w.Y * q.W + w.Z * q.X - w.X * q.Z,
183 w.Z * q.W + w.X * q.Y - w.Y * q.X,
184 -w.X * q.X - w.Y * q.Y - w.Z * q.Z);
185 }
186
187 public static Quaternion operator /(Quaternion a, float b)
188 {
189 if (b == 0) throw new DivideByZeroException();
190 return new Quaternion(a.X / b, a.Y / b, a.Z / b, a.W / b);
191 }
192
193 public static explicit operator MonoXnaCompactMaths.Quaternion(Quaternion a)
194 {
195 return new MonoXnaCompactMaths.Quaternion(a.X, a.Y, a.Z, a.W);
196 }
197 }
198}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs
new file mode 100644
index 0000000..031faab
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/TransformUtil.cs
@@ -0,0 +1,102 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25using MonoXnaCompactMaths;
26
27namespace XnaDevRu.BulletX
28{
29 public static class TransformUtil
30 {
31 //const float AngularMotionTreshold = 0.5f * MonoXnaCompactMaths.MathHelper.PiOver2;
32 const float AngularMotionTreshold = 0.5f * (float)Math.PI / 2.0f;
33
34 public static void IntegrateTransform(Matrix currentTransform, Vector3 linearVelocity, Vector3 angularVelocity, float timeStep, ref Matrix predictedTransform)
35 {
36 predictedTransform.Translation = currentTransform.Translation + linearVelocity * timeStep;
37 //exponential map
38 Vector3 axis;
39 float angle = angularVelocity.Length();
40 //limit the angular motion
41 if (angle * timeStep > AngularMotionTreshold)
42 {
43 angle = AngularMotionTreshold / timeStep;
44 }
45
46 if (angle < 0.001f)
47 {
48 // use Taylor's expansions of sync function
49 axis = angularVelocity * (0.5f * timeStep - (timeStep * timeStep * timeStep) * (0.020833333333f) * angle * angle);
50 }
51 else
52 {
53 // sync(fAngle) = sin(c*fAngle)/t
54 axis = angularVelocity * ((float)Math.Sin(0.5f * angle * timeStep) / angle);
55 }
56 Quaternion dorn = new Quaternion(axis.X, axis.Y, axis.Z, (float)Math.Cos(angle * timeStep * 0.5f));
57 Quaternion ornA = MatrixOperations.GetRotation(currentTransform);
58
59 Quaternion predictedOrn = dorn * ornA;
60 predictedOrn.Normalize();
61
62 MatrixOperations.SetRotation(ref predictedTransform, predictedOrn);
63
64 Matrix test = Matrix.CreateFromQuaternion(predictedOrn);
65 }
66
67 public static void CalculateVelocity(Matrix transformA, Matrix transformB, float timeStep, ref Vector3 linearVelocity, ref Vector3 angularVelocity)
68 {
69 linearVelocity = (transformB.Translation - transformA.Translation) / timeStep;
70 Matrix dmat = transformB * MathHelper.InvertMatrix(transformA);
71 Quaternion dorn = Quaternion.CreateFromRotationMatrix(dmat);
72
73 Vector3 axis;
74 float angle = 2 * (float)Math.Acos(dorn.W);
75 axis = new Vector3(dorn.X, dorn.Y, dorn.Z);
76 //axis[3] = 0.f;
77 //check for axis length
78 float len = axis.LengthSquared();
79 if (len < MathHelper.Epsilon * MathHelper.Epsilon)
80 axis = new Vector3(1f, 0f, 0f);
81 else
82 axis /= (float)Math.Sqrt(len);
83
84 angularVelocity = axis * angle / timeStep;
85 }
86
87 public static void CalculateDiffAxisAngle(Matrix transformA, Matrix transformB, out Vector3 axis, out float angle)
88 {
89 Matrix dmat = transformB * MathHelper.InvertMatrix(transformA);
90 Quaternion dorn = MathHelper.GetRotation(dmat);
91
92 angle = 2f * (float)Math.Acos(dorn.W);
93 axis = new Vector3(dorn.X, dorn.Y, dorn.Z);
94 //check for axis length
95 float len = axis.LengthSquared();
96 if (len < MathHelper.Epsilon * MathHelper.Epsilon)
97 axis = new Vector3(1f, 0f, 0f);
98 else
99 axis /= (float)Math.Sqrt(len);
100 }
101 }
102}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs
new file mode 100644
index 0000000..79b1262
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector3.cs
@@ -0,0 +1,221 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26namespace XnaDevRu.BulletX.LinearMath
27{
28 internal class Vector3 : QuadWord
29 {
30 public Vector3() { }
31
32 public Vector3(float x, float y, float z)
33 : base(x, y, z) { }
34
35 public void SetInterpolate3(Vector3 a, Vector3 b, float c)
36 {
37 float s = 1.0f - c;
38 X = s * a.X + c * b.X;
39 Y = s * a.Y + c * b.Y;
40 Z = s * a.Z + c * b.Z;
41 }
42
43 public float LengthSquared()
44 {
45 return Dot(this, this);
46 }
47
48 public float Length()
49 {
50 return (float)Math.Sqrt(LengthSquared());
51 }
52
53 public int MinAxis()
54 {
55 return X < Y ? (X < Z ? 0 : 2) : (Y < Z ? 1 : 2);
56 }
57
58 public int MaxAxis()
59 {
60 return X < Y ? (Y < Z ? 2 : 1) : (X < Z ? 2 : 0);
61 }
62
63 public int FurthestAxis()
64 {
65 return Absolute(this).MinAxis();
66 }
67
68 public int ClosestAxis()
69 {
70 return Absolute(this).MaxAxis();
71 }
72
73 public Vector3 Rotate(Vector3 axis, float angle)
74 {
75 Vector3 o = axis * Dot(axis, this);
76 Vector3 x = this - o;
77 Vector3 y = Cross(axis, this);
78
79 return (o + x * (float)Math.Cos(angle) + y * (float)Math.Sin(angle));
80 }
81
82 public static Vector3 Lerp(Vector3 a, Vector3 b, float c)
83 {
84 return new Vector3(
85 a.X + (b.X - a.X) * c,
86 a.Y + (b.Y - a.Y) * c,
87 a.Z + (b.Z - a.Z) * c);
88 }
89
90 public static float Angle(Vector3 a, Vector3 b)
91 {
92 float s = (float)Math.Sqrt(a.LengthSquared() * b.LengthSquared());
93 if (s == 0) throw new DivideByZeroException();
94 return (float)Math.Acos(Dot(a, b) / s);
95 }
96
97 public static Vector3 Absolute(Vector3 a)
98 {
99 return new Vector3(
100 Math.Abs(a.X),
101 Math.Abs(a.Y),
102 Math.Abs(a.Z));
103 }
104
105 public static Vector3 Normalize(Vector3 a)
106 {
107 return a / a.Length();
108 }
109
110 public static Vector3 Cross(Vector3 a, Vector3 b)
111 {
112 return new Vector3(
113 a.Y * b.Z - a.Z * b.Y,
114 a.Z * b.X - a.X * b.Z,
115 a.X * b.Y - a.Y * b.X);
116 }
117
118 public static float Dot(Vector3 a, Vector3 b)
119 {
120 return a.X * b.X + a.Y * b.Y + a.Z * b.Z;
121 }
122
123 public static float Triple(Vector3 a, Vector3 b, Vector3 c)
124 {
125 return a.X * (b.Y * c.Z - b.Z * c.Y) +
126 a.Y * (b.Z * c.X - b.X * c.Z) +
127 a.Z * (b.X * c.Y - b.Y * c.X);
128 }
129
130 public static float Distance(Vector3 a, Vector3 b)
131 {
132 return (b - a).Length();
133 }
134
135 public static float DistanceSquared(Vector3 a, Vector3 b)
136 {
137 return (b - a).LengthSquared();
138 }
139
140 public static Vector3 Rotate(Vector3 a, Vector3 axis, float angle)
141 {
142 Vector3 o = axis * Dot(axis, a);
143 Vector3 x = a - o;
144 Vector3 y = Cross(axis, a);
145
146 return (o + x * (float)Math.Cos(angle) + y * (float)Math.Sin(angle));
147 }
148
149 public static Vector3 operator +(Vector3 a, Vector3 b)
150 {
151 return new Vector3(a.X + b.X, a.Y + b.Y, a.Z + b.Z);
152 }
153
154 public static Vector3 operator -(Vector3 a, Vector3 b)
155 {
156 return new Vector3(a.X - b.X, a.Y - b.Y, a.Z - b.Z);
157 }
158
159 public static Vector3 operator -(Vector3 a)
160 {
161 return new Vector3(-a.X, -a.Y, -a.Z);
162 }
163
164 public static Vector3 operator *(float b, Vector3 a)
165 {
166 return new Vector3(a.X * b, a.Y * b, a.Z * b);
167 }
168
169 public static Vector3 operator *(Vector3 a, float b)
170 {
171 return new Vector3(a.X * b, a.Y * b, a.Z * b);
172 }
173
174 public static Vector3 operator *(Vector3 a, Vector3 b)
175 {
176 return new Vector3(a.X * b.X, a.Y * b.Y, a.Z * b.Z);
177 }
178
179 public static Vector3 operator /(Vector3 a, float b)
180 {
181 if (b == 0) throw new DivideByZeroException();
182 return new Vector3(a.X / b, a.Y / b, a.Z / b);
183 }
184
185 public static Vector3 operator /(Vector3 a, Vector3 b)
186 {
187 if (b.X == 0 || b.Y == 0 || b.Z == 0) throw new DivideByZeroException();
188 return new Vector3(a.X / b.X, a.Y / b.Y, a.Z / b.Z);
189 }
190
191 public static bool operator ==(Vector3 a, Vector3 b)
192 {
193 return a.X == b.X && a.Y == b.Y && a.Z == b.Z;
194 }
195
196 public static bool operator !=(Vector3 a, Vector3 b)
197 {
198 return a.X != b.X || a.Y != b.Y || a.Z != b.Z;
199 }
200
201 public static explicit operator MonoXnaCompactMaths.Vector3(Vector3 a)
202 {
203 return new MonoXnaCompactMaths.Vector3(a.X, a.Y, a.Z);
204 }
205
206 public override bool Equals(object obj)
207 {
208 return object.Equals(this, obj);
209 }
210
211 public override int GetHashCode()
212 {
213 return X.GetHashCode() & Y.GetHashCode() & Z.GetHashCode();
214 }
215
216 public override string ToString()
217 {
218 return string.Format("{0}, {1}, {2}", X, Y, Z);
219 }
220 }
221}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs
new file mode 100644
index 0000000..a64ce97
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/LinearMath/Vector4.cs
@@ -0,0 +1,110 @@
1/*
2 Bullet for XNA Copyright (c) 2003-2007 Vsevolod Klementjev http://www.codeplex.com/xnadevru
3 Bullet original C++ version Copyright (c) 2003-2007 Erwin Coumans http://bulletphysics.com
4
5 This software is provided 'as-is', without any express or implied
6 warranty. In no event will the authors be held liable for any damages
7 arising from the use of this software.
8
9 Permission is granted to anyone to use this software for any purpose,
10 including commercial applications, and to alter it and redistribute it
11 freely, subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software
15 in a product, an acknowledgment in the product documentation would be
16 appreciated but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20*/
21
22using System;
23using System.Collections.Generic;
24using System.Text;
25
26namespace XnaDevRu.BulletX.LinearMath
27{
28 internal class Vector4 : Vector3
29 {
30 public Vector4() { }
31
32 public Vector4(float x, float y, float z, float w)
33 : base(x, y, z) { W = w; }
34
35 public static Vector4 Absolute4(Vector4 a)
36 {
37 return new Vector4(
38 Math.Abs(a.X),
39 Math.Abs(a.Y),
40 Math.Abs(a.Z),
41 Math.Abs(a.W));
42 }
43
44 public int MaxAxis4()
45 {
46 int maxIndex = -1;
47 float maxVal = -1e30f;
48 if (X > maxVal)
49 {
50 maxIndex = 0;
51 maxVal = X;
52 }
53 if (Y > maxVal)
54 {
55 maxIndex = 1;
56 maxVal = Y;
57 }
58 if (Z > maxVal)
59 {
60 maxIndex = 2;
61 maxVal = Z;
62 }
63 if (W > maxVal)
64 {
65 maxIndex = 3;
66 maxVal = W;
67 }
68
69 return maxIndex;
70 }
71
72 public int MinAxis4()
73 {
74 int minIndex = -1;
75 float minVal = 1e30f;
76 if (X < minVal)
77 {
78 minIndex = 0;
79 minVal = X;
80 }
81 if (Y < minVal)
82 {
83 minIndex = 1;
84 minVal = Y;
85 }
86 if (Z < minVal)
87 {
88 minIndex = 2;
89 minVal = Z;
90 }
91 if (W < minVal)
92 {
93 minIndex = 3;
94 minVal = W;
95 }
96
97 return minIndex;
98 }
99
100 public int ClosestAxis4()
101 {
102 return Absolute4(this).MaxAxis4();
103 }
104
105 public static explicit operator MonoXnaCompactMaths.Vector4(Vector4 a)
106 {
107 return new MonoXnaCompactMaths.Vector4(a.X, a.Y, a.Z, a.W);
108 }
109 }
110}
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj b/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj
new file mode 100644
index 0000000..bed8d19
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Modified.XnaDevRu.BulletX.csproj
@@ -0,0 +1,168 @@
1<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
2 <PropertyGroup>
3 <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
4 <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
5 <ProductVersion>8.0.50727</ProductVersion>
6 <SchemaVersion>2.0</SchemaVersion>
7 <ProjectGuid>{44270344-ACA7-4875-B585-81D5C06D0489}</ProjectGuid>
8 <OutputType>Library</OutputType>
9 <AppDesignerFolder>Properties</AppDesignerFolder>
10 <RootNamespace>XnaDevRu.BulletX</RootNamespace>
11 <AssemblyName>Modified.XnaDevRu.BulletX</AssemblyName>
12 </PropertyGroup>
13 <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
14 <DebugSymbols>true</DebugSymbols>
15 <DebugType>full</DebugType>
16 <Optimize>false</Optimize>
17 <OutputPath>bin\Debug\</OutputPath>
18 <DefineConstants>DEBUG;TRACE</DefineConstants>
19 <ErrorReport>prompt</ErrorReport>
20 <WarningLevel>4</WarningLevel>
21 </PropertyGroup>
22 <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
23 <DebugType>pdbonly</DebugType>
24 <Optimize>true</Optimize>
25 <OutputPath>bin\Release\</OutputPath>
26 <DefineConstants>TRACE</DefineConstants>
27 <ErrorReport>prompt</ErrorReport>
28 <WarningLevel>4</WarningLevel>
29 </PropertyGroup>
30 <ItemGroup>
31 <Reference Include="System" />
32 </ItemGroup>
33 <ItemGroup>
34 <Compile Include="BulletDebug.cs" />
35 <Compile Include="Collision\BroadphaseCollision\AxisSweep3.cs" />
36 <Compile Include="Collision\BroadphaseCollision\BroadphaseNativeTypes.cs" />
37 <Compile Include="Collision\BroadphaseCollision\BroadphasePair.cs" />
38 <Compile Include="Collision\BroadphaseCollision\BroadphaseProxy.cs" />
39 <Compile Include="Collision\BroadphaseCollision\CollisionAlgorithm.cs" />
40 <Compile Include="Collision\BroadphaseCollision\CollisionAlgorithmConstructionInfo.cs" />
41 <Compile Include="Collision\BroadphaseCollision\DispatcherInfo.cs" />
42 <Compile Include="Collision\BroadphaseCollision\IBroadphase.cs" />
43 <Compile Include="Collision\BroadphaseCollision\IDispatcher.cs" />
44 <Compile Include="Collision\BroadphaseCollision\IOverlapCallback.cs" />
45 <Compile Include="Collision\BroadphaseCollision\OverlappingPairCache.cs" />
46 <Compile Include="Collision\BroadphaseCollision\SimpleBroadphase.cs" />
47 <Compile Include="Collision\BroadphaseCollision\SimpleBroadphaseProxy.cs" />
48 <Compile Include="Collision\CollisionDispatch\BridgeTriangleRaycastCallback.cs" />
49 <Compile Include="Collision\CollisionDispatch\CollisionAlgorithmCreateFunc.cs" />
50 <Compile Include="Collision\CollisionDispatch\CollisionDispatcher.cs" />
51 <Compile Include="Collision\CollisionDispatch\CollisionObject.cs" />
52 <Compile Include="Collision\CollisionDispatch\CollisionPairCallback.cs" />
53 <Compile Include="Collision\CollisionDispatch\CollisionWorld.cs" />
54 <Compile Include="Collision\CollisionDispatch\CompoundCollisionAlgorithm.cs" />
55 <Compile Include="Collision\CollisionDispatch\ConvexConcaveCollisionAlgorithm.cs" />
56 <Compile Include="Collision\CollisionDispatch\ConvexConvexCollisionAlgorithm.cs" />
57 <Compile Include="Collision\CollisionDispatch\ConvexTriangleCallback.cs" />
58 <Compile Include="Collision\CollisionDispatch\EmptyAlgorithm.cs" />
59 <Compile Include="Collision\CollisionDispatch\ManifoldResult.cs" />
60 <Compile Include="Collision\CollisionDispatch\SimulationIslandManager.cs" />
61 <Compile Include="Collision\CollisionDispatch\SphereBoxCollisionAlgorithm.cs" />
62 <Compile Include="Collision\CollisionDispatch\SphereSphereCollisionAlgorithm.cs" />
63 <Compile Include="Collision\CollisionDispatch\SphereTriangleCollisionAlgorithm.cs" />
64 <Compile Include="Collision\CollisionDispatch\SphereTriangleDetector.cs" />
65 <Compile Include="Collision\CollisionDispatch\UnionFind.cs" />
66 <Compile Include="Collision\CollisionShapes\BoxShape.cs" />
67 <Compile Include="Collision\CollisionShapes\BUSimplex1to4.cs" />
68 <Compile Include="Collision\CollisionShapes\BvhTriangleMeshShape.cs" />
69 <Compile Include="Collision\CollisionShapes\CollisionShape.cs" />
70 <Compile Include="Collision\CollisionShapes\CompoundShape.cs" />
71 <Compile Include="Collision\CollisionShapes\ConcaveShape.cs" />
72 <Compile Include="Collision\CollisionShapes\ConeShape.cs" />
73 <Compile Include="Collision\CollisionShapes\ConvexHullShape.cs" />
74 <Compile Include="Collision\CollisionShapes\ConvexShape.cs" />
75 <Compile Include="Collision\CollisionShapes\ConvexTriangleMeshShape.cs" />
76 <Compile Include="Collision\CollisionShapes\CylinderShape.cs" />
77 <Compile Include="Collision\CollisionShapes\CylinderShapeX.cs" />
78 <Compile Include="Collision\CollisionShapes\CylinderShapeZ.cs" />
79 <Compile Include="Collision\CollisionShapes\EmptyShape.cs" />
80 <Compile Include="Collision\CollisionShapes\FilteredCallback.cs" />
81 <Compile Include="Collision\CollisionShapes\InternalTriangleIndexCallback.cs" />
82 <Compile Include="Collision\CollisionShapes\LocalSupportVertexCallback.cs" />
83 <Compile Include="Collision\CollisionShapes\MinkowskiSumShape.cs" />
84 <Compile Include="Collision\CollisionShapes\MultiSphereShape.cs" />
85 <Compile Include="Collision\CollisionShapes\NodeOverlapCallback.cs" />
86 <Compile Include="Collision\CollisionShapes\OptimizedBvh.cs" />
87 <Compile Include="Collision\CollisionShapes\OptimizedBvhNode.cs" />
88 <Compile Include="Collision\CollisionShapes\PolyhedralConvexShape.cs" />
89 <Compile Include="Collision\CollisionShapes\SphereShape.cs" />
90 <Compile Include="Collision\CollisionShapes\StaticPlaneShape.cs" />
91 <Compile Include="Collision\CollisionShapes\StridingMeshInterface.cs" />
92 <Compile Include="Collision\CollisionShapes\SupportVertexCallback.cs" />
93 <Compile Include="Collision\CollisionShapes\TriangleBuffer.cs" />
94 <Compile Include="Collision\CollisionShapes\TriangleCallback.cs" />
95 <Compile Include="Collision\CollisionShapes\TriangleIndexVertexArray.cs" />
96 <Compile Include="Collision\CollisionShapes\TriangleMesh.cs" />
97 <Compile Include="Collision\CollisionShapes\TriangleMeshShape.cs" />
98 <Compile Include="Collision\CollisionShapes\TriangleShape.cs" />
99 <Compile Include="Collision\NarrowPhaseCollision\ContinuousConvexCollision.cs" />
100 <Compile Include="Collision\NarrowPhaseCollision\ConvexCast.cs" />
101 <Compile Include="Collision\NarrowPhaseCollision\DiscreteCollisionDetectorInterface.cs" />
102 <Compile Include="Collision\NarrowPhaseCollision\GjkConvexCast.cs" />
103 <Compile Include="Collision\NarrowPhaseCollision\GjkEpa.cs" />
104 <Compile Include="Collision\NarrowPhaseCollision\GjkEpaPenetrationDepthSolver.cs" />
105 <Compile Include="Collision\NarrowPhaseCollision\GjkEpaSolver.cs" />
106 <Compile Include="Collision\NarrowPhaseCollision\GjkPairDetector.cs" />
107 <Compile Include="Collision\NarrowPhaseCollision\IConvexPenetrationDepthSolver.cs" />
108 <Compile Include="Collision\NarrowPhaseCollision\ISimplexSolver.cs" />
109 <Compile Include="Collision\NarrowPhaseCollision\ManifoldPoint.cs" />
110 <Compile Include="Collision\NarrowPhaseCollision\MinkowskiPenetrationDepthSolver.cs" />
111 <Compile Include="Collision\NarrowPhaseCollision\PersistentManifold.cs" />
112 <Compile Include="Collision\NarrowPhaseCollision\PointCollector.cs" />
113 <Compile Include="Collision\NarrowPhaseCollision\SubsimplexConvexCast.cs" />
114 <Compile Include="Collision\NarrowPhaseCollision\TriangleRaycastCallback.cs" />
115 <Compile Include="Collision\NarrowPhaseCollision\VoronoiSimplexSolver.cs" />
116 <Compile Include="Dynamics\ConstraintSolver\ContactConstraint.cs" />
117 <Compile Include="Dynamics\ConstraintSolver\ContactSolverInfo.cs" />
118 <Compile Include="Dynamics\ConstraintSolver\Generic6DofConstraint.cs" />
119 <Compile Include="Dynamics\ConstraintSolver\HingeConstraint.cs" />
120 <Compile Include="Dynamics\ConstraintSolver\IConstraintSolver.cs" />
121 <Compile Include="Dynamics\ConstraintSolver\JacobianEntry.cs" />
122 <Compile Include="Dynamics\ConstraintSolver\Point2PointConstraint.cs" />
123 <Compile Include="Dynamics\ConstraintSolver\SequentialImpulseConstraintSolver.cs" />
124 <Compile Include="Dynamics\ConstraintSolver\Solve2LinearConstraint.cs" />
125 <Compile Include="Dynamics\ConstraintSolver\SolverBody.cs" />
126 <Compile Include="Dynamics\ConstraintSolver\SolverConstraint.cs" />
127 <Compile Include="Dynamics\ConstraintSolver\TypedConstraint.cs" />
128 <Compile Include="Dynamics\DiscreteDynamicsWorld.cs" />
129 <Compile Include="Dynamics\DynamicsWorld.cs" />
130 <Compile Include="Dynamics\RigidBody.cs" />
131 <Compile Include="Dynamics\SimpleDynamicsWorld.cs" />
132 <Compile Include="Dynamics\Vehicle\RaycastVehicle.cs" />
133 <Compile Include="Dynamics\Vehicle\VehicleRaycaster.cs" />
134 <Compile Include="Dynamics\Vehicle\WheelInfo.cs" />
135 <Compile Include="Exceptions\BulletException.cs" />
136 <Compile Include="IDebugDraw.cs" />
137 <Compile Include="LinearMath\DefaultMotionState.cs" />
138 <Compile Include="LinearMath\MathHelper.cs" />
139 <Compile Include="LinearMath\MatrixOperations.cs" />
140 <Compile Include="LinearMath\MotionState.cs" />
141 <Compile Include="LinearMath\QuadWord.cs" />
142 <Compile Include="LinearMath\Quaternion.cs" />
143 <Compile Include="LinearMath\TransformUtil.cs" />
144 <Compile Include="LinearMath\Vector3.cs" />
145 <Compile Include="LinearMath\Vector4.cs" />
146 <Compile Include="Properties\AssemblyInfo.cs" />
147 </ItemGroup>
148 <ItemGroup>
149 <ProjectReference Include="..\MonoXnaCompactMaths\MonoXnaCompactMaths.csproj">
150 <Project>{121147BC-B06B-406C-84E9-907F268CF0EB}</Project>
151 <Name>MonoXnaCompactMaths</Name>
152 </ProjectReference>
153 </ItemGroup>
154 <ItemGroup>
155 <None Include="BulletX.snk" />
156 </ItemGroup>
157 <ItemGroup>
158 <Folder Include="Content\" />
159 </ItemGroup>
160 <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />
161 <!-- To modify your build process, add your task inside one of the targets below and uncomment it.
162 Other similar extension points exist, see Microsoft.Common.targets.
163 <Target Name="BeforeBuild">
164 </Target>
165 <Target Name="AfterBuild">
166 </Target>
167 -->
168</Project> \ No newline at end of file
diff --git a/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs b/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs
new file mode 100644
index 0000000..efc8fd6
--- /dev/null
+++ b/libraries/ModifiedBulletX/ModifiedBulletX/Properties/AssemblyInfo.cs
@@ -0,0 +1,35 @@
1using System.Reflection;
2using System.Runtime.CompilerServices;
3using System.Runtime.InteropServices;
4
5// La información general sobre un ensamblado se controla mediante el siguiente
6// conjunto de atributos. Cambie estos atributos para modificar la información
7// asociada con un ensamblado.
8[assembly: AssemblyTitle("Modified.XnaDevRu.BulletX")]
9[assembly: AssemblyDescription("Modified Bullet for XNA")]
10[assembly: AssemblyConfiguration("")]
11[assembly: AssemblyCompany("XNADev.ru")]
12[assembly: AssemblyProduct("Bullet for XNA")]
13[assembly: AssemblyCopyright("Copyright © 2007 XNADev.ru")]
14[assembly: AssemblyTrademark("")]
15[assembly: AssemblyCulture("")]
16
17// Si establece ComVisible como false hace que los tipos de este ensamblado no sean visibles
18// a los componentes COM. Si necesita obtener acceso a un tipo en este ensamblado desde
19// COM, establezca el atributo ComVisible como true en este tipo.
20[assembly: ComVisible(false)]
21
22// El siguiente GUID sirve como identificador de la biblioteca de tipos si este proyecto se expone a COM
23[assembly: Guid("a6148224-0271-4cd8-9d0a-5d4955cd2b2c")]
24
25// La información de versión de un ensamblado consta de los cuatro valores siguientes:
26//
27// Versión principal
28// Versión secundaria
29// Número de versión de compilación
30// Revisión
31//
32// Puede especificar todos los valores o puede establecer como valores predeterminados los números de revisión y generación
33// mediante el asterisco ('*'), como se muestra a continuación:
34[assembly: AssemblyVersion("1.0.0.0")]
35[assembly: AssemblyFileVersion("1.0.0.0")]
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs
new file mode 100644
index 0000000..a104d89
--- /dev/null
+++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Matrix.cs
@@ -0,0 +1,683 @@
1#region License
2/*
3MIT License
4Copyright © 2006 The Mono.Xna Team
5
6All rights reserved.
7
8Permission is hereby granted, free of charge, to any person obtaining a copy
9of this software and associated documentation files (the "Software"), to deal
10in the Software without restriction, including without limitation the rights
11to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12copies of the Software, and to permit persons to whom the Software is
13furnished to do so, subject to the following conditions:
14
15The above copyright notice and this permission notice shall be included in all
16copies or substantial portions of the Software.
17
18THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
23OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
24SOFTWARE.
25*/
26#endregion License
27
28using System;
29using System.ComponentModel;
30using System.Runtime.InteropServices;
31
32namespace MonoXnaCompactMaths
33{
34 [Serializable]
35 [StructLayout(LayoutKind.Sequential)]
36 //[TypeConverter(typeof(MatrixConverter))]
37 public struct Matrix : IEquatable<Matrix>
38 {
39 #region Public Constructors
40
41 public Matrix(float m11, float m12, float m13, float m14, float m21, float m22, float m23, float m24, float m31,
42 float m32, float m33, float m34, float m41, float m42, float m43, float m44)
43 {
44 this.M11 = m11;
45 this.M12 = m12;
46 this.M13 = m13;
47 this.M14 = m14;
48 this.M21 = m21;
49 this.M22 = m22;
50 this.M23 = m23;
51 this.M24 = m24;
52 this.M31 = m31;
53 this.M32 = m32;
54 this.M33 = m33;
55 this.M34 = m34;
56 this.M41 = m41;
57 this.M42 = m42;
58 this.M43 = m43;
59 this.M44 = m44;
60 }
61
62 #endregion Public Constructors
63
64
65 #region Public Fields
66
67 public float M11;
68 public float M12;
69 public float M13;
70 public float M14;
71 public float M21;
72 public float M22;
73 public float M23;
74 public float M24;
75 public float M31;
76 public float M32;
77 public float M33;
78 public float M34;
79 public float M41;
80 public float M42;
81 public float M43;
82 public float M44;
83
84 #endregion Public Fields
85
86
87 #region Private Members
88 private static Matrix identity = new Matrix(1f, 0f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 0f, 1f, 0f, 0f, 0f, 0f, 1f);
89 #endregion Private Members
90
91
92 #region Public Properties
93
94 public Vector3 Backward
95 {
96 get
97 {
98 return new Vector3(this.M31, this.M32, this.M33);
99 }
100 set
101 {
102 this.M31 = value.X;
103 this.M32 = value.Y;
104 this.M33 = value.Z;
105 }
106 }
107
108
109 public Vector3 Down
110 {
111 get
112 {
113 return new Vector3(-this.M21, -this.M22, -this.M23);
114 }
115 set
116 {
117 this.M21 = -value.X;
118 this.M22 = -value.Y;
119 this.M23 = -value.Z;
120 }
121 }
122
123
124 public Vector3 Forward
125 {
126 get
127 {
128 return new Vector3(-this.M31, -this.M32, -this.M33);
129 }
130 set
131 {
132 this.M31 = -value.X;
133 this.M32 = -value.Y;
134 this.M33 = -value.Z;
135 }
136 }
137
138
139 public static Matrix Identity
140 {
141 get { return identity; }
142 }
143
144
145 public Vector3 Left
146 {
147 get
148 {
149 return new Vector3(-this.M11, -this.M12, -this.M13);
150 }
151 set
152 {
153 this.M11 = -value.X;
154 this.M12 = -value.Y;
155 this.M13 = -value.Z;
156 }
157 }
158
159
160 public Vector3 Right
161 {
162 get
163 {
164 return new Vector3(this.M11, this.M12, this.M13);
165 }
166 set
167 {
168 this.M11 = value.X;
169 this.M12 = value.Y;
170 this.M13 = value.Z;
171 }
172 }
173
174
175 public Vector3 Translation
176 {
177 get
178 {
179 return new Vector3(this.M41, this.M42, this.M43);
180 }
181 set
182 {
183 this.M41 = value.X;
184 this.M42 = value.Y;
185 this.M43 = value.Z;
186 }
187 }
188
189
190 public Vector3 Up
191 {
192 get
193 {
194 return new Vector3(this.M21, this.M22, this.M23);
195 }
196 set
197 {
198 this.M21 = value.X;
199 this.M22 = value.Y;
200 this.M23 = value.Z;
201 }
202 }
203 #endregion Public Properties
204
205
206 #region Public Methods
207
208 public static Matrix Add(Matrix matrix1, Matrix matrix2)
209 {
210 throw new NotImplementedException();
211 }
212
213
214 public static void Add(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)
215 {
216 throw new NotImplementedException();
217 }
218
219
220 public static Matrix CreateBillboard(Vector3 objectPosition, Vector3 cameraPosition,
221 Vector3 cameraUpVector, Nullable<Vector3> cameraForwardVector)
222 {
223 throw new NotImplementedException();
224 }
225
226
227 public static void CreateBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition,
228 ref Vector3 cameraUpVector, Vector3? cameraForwardVector, out Matrix result)
229 {
230 throw new NotImplementedException();
231 }
232
233
234 public static Matrix CreateConstrainedBillboard(Vector3 objectPosition, Vector3 cameraPosition,
235 Vector3 rotateAxis, Nullable<Vector3> cameraForwardVector, Nullable<Vector3> objectForwardVector)
236 {
237 throw new NotImplementedException();
238 }
239
240
241 public static void CreateConstrainedBillboard(ref Vector3 objectPosition, ref Vector3 cameraPosition,
242 ref Vector3 rotateAxis, Vector3? cameraForwardVector, Vector3? objectForwardVector, out Matrix result)
243 {
244 throw new NotImplementedException();
245 }
246
247
248 public static Matrix CreateFromAxisAngle(Vector3 axis, float angle)
249 {
250 throw new NotImplementedException();
251 }
252
253
254 public static void CreateFromAxisAngle(ref Vector3 axis, float angle, out Matrix result)
255 {
256 throw new NotImplementedException();
257 }
258
259
260 public static Matrix CreateFromQuaternion(Quaternion quaternion)
261 {
262 //---
263 //http://lists.ximian.com/pipermail/mono-patches/2006-December/084667.html
264 float xx = quaternion.X * quaternion.X;
265 float xy = quaternion.X * quaternion.Y;
266 float xw = quaternion.X * quaternion.W;
267 float yy = quaternion.Y * quaternion.Y;
268 float yw = quaternion.Y * quaternion.W;
269 float yz = quaternion.Y * quaternion.Z;
270 float zx = quaternion.Z * quaternion.X;
271 float zw = quaternion.Z * quaternion.W;
272 float zz = quaternion.Z * quaternion.Z;
273 return new Matrix(1f - (2f * (yy + zz)), 2f * (xy + zw), 2f * (zx - yw), 0f, 2f * (xy - zw),
274 1f - (2f * (zz + xx)), 2f * (yz + xw), 0f, 2f * (zx + yw), 2f * (yz - xw),
275 1f - (2f * (yy + xx)), 0f, 0f, 0f, 0f, 1f);
276 //throw new NotImplementedException();
277 }
278
279
280 public static void CreateFromQuaternion(ref Quaternion quaternion, out Matrix result)
281 {
282 throw new NotImplementedException();
283 }
284
285
286 public static Matrix CreateLookAt(Vector3 cameraPosition, Vector3 cameraTarget, Vector3 cameraUpVector)
287 {
288 throw new NotImplementedException();
289 }
290
291
292 public static void CreateLookAt(ref Vector3 cameraPosition, ref Vector3 cameraTarget, ref Vector3 cameraUpVector, out Matrix result)
293 {
294 throw new NotImplementedException();
295 }
296
297
298 public static Matrix CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane)
299 {
300 throw new NotImplementedException();
301 }
302
303
304 public static void CreateOrthographic(float width, float height, float zNearPlane, float zFarPlane, out Matrix result)
305 {
306 throw new NotImplementedException();
307 }
308
309
310 public static Matrix CreateOrthographicOffCenter(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane)
311 {
312 throw new NotImplementedException();
313 }
314
315
316 public static void CreateOrthographicOffCenter(float left, float right, float bottom, float top,
317 float zNearPlane, float zFarPlane, out Matrix result)
318 {
319 throw new NotImplementedException();
320 }
321
322
323 public static Matrix CreatePerspective(float width, float height, float zNearPlane, float zFarPlane)
324 {
325 throw new NotImplementedException();
326 }
327
328
329 public static void CreatePerspective(float width, float height, float zNearPlane, float zFarPlane, out Matrix result)
330 {
331 throw new NotImplementedException();
332 }
333
334
335 public static Matrix CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float zNearPlane, float zFarPlane)
336 {
337 throw new NotImplementedException();
338 }
339
340
341 public static void CreatePerspectiveFieldOfView(float fieldOfView, float aspectRatio, float nearPlaneDistance, float farPlaneDistance, out Matrix result)
342 {
343 throw new NotImplementedException();
344 }
345
346
347 public static Matrix CreatePerspectiveOffCenter(float left, float right, float bottom, float top, float zNearPlane, float zFarPlane)
348 {
349 throw new NotImplementedException();
350 }
351
352
353 public static void CreatePerspectiveOffCenter(float left, float right, float bottom, float top, float nearPlaneDistance, float farPlaneDistance, out Matrix result)
354 {
355 throw new NotImplementedException();
356 }
357
358
359 public static Matrix CreateRotationX(float radians)
360 {
361 throw new NotImplementedException();
362 }
363
364
365 public static void CreateRotationX(float radians, out Matrix result)
366 {
367 throw new NotImplementedException();
368 }
369
370
371 public static Matrix CreateRotationY(float radians)
372 {
373 throw new NotImplementedException();
374 }
375
376
377 public static void CreateRotationY(float radians, out Matrix result)
378 {
379 throw new NotImplementedException();
380 }
381
382
383 public static Matrix CreateRotationZ(float radians)
384 {
385 throw new NotImplementedException();
386 }
387
388
389 public static void CreateRotationZ(float radians, out Matrix result)
390 {
391 throw new NotImplementedException();
392 }
393
394
395 public static Matrix CreateScale(float scale)
396 {
397 throw new NotImplementedException();
398 }
399
400
401 public static void CreateScale(float scale, out Matrix result)
402 {
403 throw new NotImplementedException();
404 }
405
406
407 public static Matrix CreateScale(float xScale, float yScale, float zScale)
408 {
409 throw new NotImplementedException();
410 }
411
412
413 public static void CreateScale(float xScale, float yScale, float zScale, out Matrix result)
414 {
415 throw new NotImplementedException();
416 }
417
418
419 public static Matrix CreateScale(Vector3 scales)
420 {
421 throw new NotImplementedException();
422 }
423
424
425 public static void CreateScale(ref Vector3 scales, out Matrix result)
426 {
427 throw new NotImplementedException();
428 }
429
430
431 public static Matrix CreateTranslation(float xPosition, float yPosition, float zPosition)
432 {
433 throw new NotImplementedException();
434 }
435
436
437 public static void CreateTranslation(ref Vector3 position, out Matrix result)
438 {
439 throw new NotImplementedException();
440 }
441
442
443 public static Matrix CreateTranslation(Vector3 position)
444 {
445 throw new NotImplementedException();
446 }
447
448
449 public static void CreateTranslation(float xPosition, float yPosition, float zPosition, out Matrix result)
450 {
451 throw new NotImplementedException();
452 }
453
454
455 public float Determinant()
456 {
457 throw new NotImplementedException();
458 }
459
460
461 public static Matrix Divide(Matrix matrix1, Matrix matrix2)
462 {
463 throw new NotImplementedException();
464 }
465
466
467 public static void Divide(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)
468 {
469 throw new NotImplementedException();
470 }
471
472
473 public static Matrix Divide(Matrix matrix1, float divider)
474 {
475 throw new NotImplementedException();
476 }
477
478
479 public static void Divide(ref Matrix matrix1, float divider, out Matrix result)
480 {
481 throw new NotImplementedException();
482 }
483
484
485 public bool Equals(Matrix other)
486 {
487 throw new NotImplementedException();
488 }
489
490
491 public override bool Equals(object obj)
492 {
493 throw new NotImplementedException();
494 }
495
496
497 public override int GetHashCode()
498 {
499 throw new NotImplementedException();
500 }
501
502
503 public static Matrix Invert(Matrix matrix)
504 {
505 throw new NotImplementedException();
506 }
507
508
509 public static void Invert(ref Matrix matrix, out Matrix result)
510 {
511 throw new NotImplementedException();
512 }
513
514
515 public static Matrix Lerp(Matrix matrix1, Matrix matrix2, float amount)
516 {
517 throw new NotImplementedException();
518 }
519
520
521 public static void Lerp(ref Matrix matrix1, ref Matrix matrix2, float amount, out Matrix result)
522 {
523 throw new NotImplementedException();
524 }
525
526 public static Matrix Multiply(Matrix matrix1, Matrix matrix2)
527 {
528 throw new NotImplementedException();
529 }
530
531
532 public static void Multiply(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)
533 {
534 throw new NotImplementedException();
535 }
536
537
538 public static Matrix Multiply(Matrix matrix1, float factor)
539 {
540 throw new NotImplementedException();
541 }
542
543
544 public static void Multiply(ref Matrix matrix1, float factor, out Matrix result)
545 {
546 throw new NotImplementedException();
547 }
548
549
550 public static Matrix Negate(Matrix matrix)
551 {
552 throw new NotImplementedException();
553 }
554
555
556 public static void Negate(ref Matrix matrix, out Matrix result)
557 {
558 throw new NotImplementedException();
559 }
560
561
562 public static Matrix operator +(Matrix matrix1, Matrix matrix2)
563 {
564 throw new NotImplementedException();
565 }
566
567
568 public static Matrix operator /(Matrix matrix1, Matrix matrix2)
569 {
570 throw new NotImplementedException();
571 }
572
573
574 public static Matrix operator /(Matrix matrix1, float divider)
575 {
576 throw new NotImplementedException();
577 }
578
579
580 public static bool operator ==(Matrix matrix1, Matrix matrix2)
581 {
582 throw new NotImplementedException();
583 }
584
585
586 public static bool operator !=(Matrix matrix1, Matrix matrix2)
587 {
588 throw new NotImplementedException();
589 }
590
591
592 public static Matrix operator *(Matrix matrix1, Matrix matrix2)
593 {
594 //---
595 float[, ] arrayMatrix1 = new float[4, 4];
596 float[, ] arrayMatrix2 = new float[4, 4];
597 float[, ] arrayMatrixProduct = new float[4, 4];
598 arrayMatrix1[0, 0] = matrix1.M11; arrayMatrix1[0, 1] = matrix1.M12; arrayMatrix1[0, 2] = matrix1.M13; arrayMatrix1[0, 3] = matrix1.M14;
599 arrayMatrix1[1, 0] = matrix1.M21; arrayMatrix1[1, 1] = matrix1.M22; arrayMatrix1[1, 2] = matrix1.M23; arrayMatrix1[1, 3] = matrix1.M24;
600 arrayMatrix1[2, 0] = matrix1.M31; arrayMatrix1[2, 1] = matrix1.M32; arrayMatrix1[2, 2] = matrix1.M33; arrayMatrix1[2, 3] = matrix1.M34;
601 arrayMatrix1[3, 0] = matrix1.M41; arrayMatrix1[3, 1] = matrix1.M42; arrayMatrix1[3, 2] = matrix1.M43; arrayMatrix1[3, 3] = matrix1.M44;
602
603 arrayMatrix2[0, 0] = matrix2.M11; arrayMatrix2[0, 1] = matrix2.M12; arrayMatrix2[0, 2] = matrix2.M13; arrayMatrix2[0, 3] = matrix2.M14;
604 arrayMatrix2[1, 0] = matrix2.M21; arrayMatrix2[1, 1] = matrix2.M22; arrayMatrix2[1, 2] = matrix2.M23; arrayMatrix2[1, 3] = matrix2.M24;
605 arrayMatrix2[2, 0] = matrix2.M31; arrayMatrix2[2, 1] = matrix2.M32; arrayMatrix2[2, 2] = matrix2.M33; arrayMatrix2[2, 3] = matrix2.M34;
606 arrayMatrix2[3, 0] = matrix2.M41; arrayMatrix2[3, 1] = matrix2.M42; arrayMatrix2[3, 2] = matrix2.M43; arrayMatrix2[3, 3] = matrix2.M44;
607
608 int n = 4;
609 for (int i = 0; i < n; i++)
610 {
611 for (int j = 0; j < n; j++)
612 {
613 // (AB)[i,j] = Sum(k=0; k < 4; k++) { A[i,k] * B[k, j] }
614 for (int k = 0; k < n; k++)
615 {
616 arrayMatrixProduct[i, j] += arrayMatrix1[i, k] * arrayMatrix2[k, j];
617 }
618 }
619 }
620 return new Matrix( arrayMatrixProduct[0, 0], arrayMatrixProduct[0, 1], arrayMatrixProduct[0, 2], arrayMatrixProduct[0, 3],
621 arrayMatrixProduct[1, 0], arrayMatrixProduct[1, 1], arrayMatrixProduct[1, 2], arrayMatrixProduct[1, 3],
622 arrayMatrixProduct[2, 0], arrayMatrixProduct[2, 1], arrayMatrixProduct[2, 2], arrayMatrixProduct[2, 3],
623 arrayMatrixProduct[3, 1], arrayMatrixProduct[3, 1], arrayMatrixProduct[3, 2], arrayMatrixProduct[3, 3]);
624 //---
625 //throw new NotImplementedException();
626 }
627
628
629 public static Matrix operator *(Matrix matrix, float scaleFactor)
630 {
631 throw new NotImplementedException();
632 }
633
634
635 public static Matrix operator -(Matrix matrix1, Matrix matrix2)
636 {
637 throw new NotImplementedException();
638 }
639
640
641 public static Matrix operator -(Matrix matrix1)
642 {
643 throw new NotImplementedException();
644 }
645
646
647 public static Matrix Subtract(Matrix matrix1, Matrix matrix2)
648 {
649 throw new NotImplementedException();
650 }
651
652
653 public static void Subtract(ref Matrix matrix1, ref Matrix matrix2, out Matrix result)
654 {
655 throw new NotImplementedException();
656 }
657
658
659 public override string ToString()
660 {
661 throw new NotImplementedException();
662 }
663
664
665 public static Matrix Transpose(Matrix matrix)
666 {
667 //---
668 return new Matrix( matrix.M11, matrix.M21, matrix.M31, matrix.M41,
669 matrix.M12, matrix.M22, matrix.M32, matrix.M42,
670 matrix.M13, matrix.M23, matrix.M33, matrix.M43,
671 matrix.M14, matrix.M24, matrix.M34, matrix.M44);
672 //---
673 //throw new NotImplementedException();
674 }
675
676
677 public static void Transpose(ref Matrix matrix, out Matrix result)
678 {
679 throw new NotImplementedException();
680 }
681 #endregion Public Methods
682 }
683}
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/MonoXnaCompactMaths.csproj b/libraries/ModifiedBulletX/MonoXnaCompactMaths/MonoXnaCompactMaths.csproj
new file mode 100644
index 0000000..c3ec4dd
--- /dev/null
+++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/MonoXnaCompactMaths.csproj
@@ -0,0 +1,51 @@
1<?xml version="1.0" encoding="utf-8"?>
2<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
3 <PropertyGroup>
4 <Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
5 <Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
6 <ProductVersion>8.0.50727</ProductVersion>
7 <SchemaVersion>2.0</SchemaVersion>
8 <ProjectGuid>{121147BC-B06B-406C-84E9-907F268CF0EB}</ProjectGuid>
9 <OutputType>Library</OutputType>
10 <AppDesignerFolder>Properties</AppDesignerFolder>
11 <RootNamespace>MonoXnaCompactMaths</RootNamespace>
12 <AssemblyName>MonoXnaCompactMaths</AssemblyName>
13 <StartupObject>
14 </StartupObject>
15 </PropertyGroup>
16 <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
17 <DebugSymbols>true</DebugSymbols>
18 <DebugType>full</DebugType>
19 <Optimize>false</Optimize>
20 <OutputPath>bin\Debug\</OutputPath>
21 <DefineConstants>DEBUG;TRACE</DefineConstants>
22 <ErrorReport>prompt</ErrorReport>
23 <WarningLevel>4</WarningLevel>
24 </PropertyGroup>
25 <PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
26 <DebugType>pdbonly</DebugType>
27 <Optimize>true</Optimize>
28 <OutputPath>bin\Release\</OutputPath>
29 <DefineConstants>TRACE</DefineConstants>
30 <ErrorReport>prompt</ErrorReport>
31 <WarningLevel>4</WarningLevel>
32 </PropertyGroup>
33 <Import Project="$(MSBuildBinPath)\Microsoft.CSharp.targets" />
34 <!-- To modify your build process, add your task inside one of the targets below and uncomment it.
35 Other similar extension points exist, see Microsoft.Common.targets.
36 <Target Name="BeforeBuild">
37 </Target>
38 <Target Name="AfterBuild">
39 </Target>
40 -->
41 <ItemGroup>
42 <Compile Include="Matrix.cs" />
43 <Compile Include="Properties\AssemblyInfo.cs" />
44 <Compile Include="Quaternion.cs" />
45 <Compile Include="Vector3.cs" />
46 <Compile Include="Vector4.cs" />
47 </ItemGroup>
48 <ItemGroup>
49 <Reference Include="System" />
50 </ItemGroup>
51</Project> \ No newline at end of file
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Properties/AssemblyInfo.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Properties/AssemblyInfo.cs
new file mode 100644
index 0000000..748b94a
--- /dev/null
+++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Properties/AssemblyInfo.cs
@@ -0,0 +1,33 @@
1using System.Reflection;
2using System.Runtime.CompilerServices;
3using System.Runtime.InteropServices;
4
5// La información general sobre un ensamblado se controla mediante el siguiente
6// conjunto de atributos. Cambie estos atributos para modificar la información
7// asociada con un ensamblado.
8[assembly: AssemblyTitle("MonoXnaCompactMaths")]
9[assembly: AssemblyDescription("MonoXnaCompactMaths (Vector3, Vector4, Quaternion and Matrix)")]
10[assembly: AssemblyConfiguration("")]
11[assembly: AssemblyCompany("")]
12[assembly: AssemblyProduct("")]
13[assembly: AssemblyCopyright("Copyright 2006 - 2007 The Mono.Xna Team")]
14[assembly: AssemblyTrademark("")]
15[assembly: AssemblyCulture("")]
16
17// Si establece ComVisible como false hace que los tipos de este ensamblado no sean visibles
18// a los componentes COM. Si necesita obtener acceso a un tipo en este ensamblado desde
19// COM, establezca el atributo ComVisible como true en este tipo.
20[assembly: ComVisible(false)]
21
22// El siguiente GUID sirve como identificador de la biblioteca de tipos si este proyecto se expone a COM
23[assembly: Guid("79f9dfd1-60f8-44b8-ab08-6f6e341976b7")]
24
25// La información de versión de un ensamblado consta de los cuatro valores siguientes:
26//
27// Versión principal
28// Versión secundaria
29// Número de versión de compilación
30// Revisión
31//
32[assembly: AssemblyVersion("1.0.0.0")]
33[assembly: AssemblyFileVersion("1.0.0.0")]
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs
new file mode 100644
index 0000000..6ed1443
--- /dev/null
+++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Quaternion.cs
@@ -0,0 +1,346 @@
1#region License
2/*
3MIT License
4Copyright © 2006 The Mono.Xna Team
5
6All rights reserved.
7
8Permission is hereby granted, free of charge, to any person obtaining a copy
9of this software and associated documentation files (the "Software"), to deal
10in the Software without restriction, including without limitation the rights
11to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12copies of the Software, and to permit persons to whom the Software is
13furnished to do so, subject to the following conditions:
14
15The above copyright notice and this permission notice shall be included in all
16copies or substantial portions of the Software.
17
18THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
23OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
24SOFTWARE.
25*/
26#endregion License
27
28using System;
29using System.ComponentModel;
30
31namespace MonoXnaCompactMaths
32{
33 [Serializable]
34 //[TypeConverter(typeof(QuaternionConverter))]
35 public struct Quaternion : IEquatable<Quaternion>
36 {
37 public float X;
38 public float Y;
39 public float Z;
40 public float W;
41 static Quaternion identity = new Quaternion(0, 0, 0, 1);
42
43
44 public Quaternion(float x, float y, float z, float w)
45 {
46 this.X = x;
47 this.Y = y;
48 this.Z = z;
49 this.W = w;
50 }
51
52
53 public Quaternion(Vector3 vectorPart, float scalarPart)
54 {
55 this.X = vectorPart.X;
56 this.Y = vectorPart.Y;
57 this.Z = vectorPart.Z;
58 this.W = scalarPart;
59 }
60
61 public static Quaternion Identity
62 {
63 get{ return identity; }
64 }
65
66
67 public static Quaternion Add(Quaternion quaternion1, Quaternion quaternion2)
68 {
69 throw new NotImplementedException();
70 }
71
72
73 public static void Add(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
74 {
75 throw new NotImplementedException();
76 }
77
78
79 public static Quaternion CreateFromAxisAngle(Vector3 axis, float angle)
80 {
81 throw new NotImplementedException();
82 }
83
84
85 public static void CreateFromAxisAngle(ref Vector3 axis, float angle, out Quaternion result)
86 {
87 throw new NotImplementedException();
88 }
89
90
91 public static Quaternion CreateFromRotationMatrix(Matrix matrix)
92 {
93 throw new NotImplementedException();
94 }
95
96
97 public static void CreateFromRotationMatrix(ref Matrix matrix, out Quaternion result)
98 {
99 throw new NotImplementedException();
100 }
101
102
103 public static Quaternion Divide(Quaternion quaternion1, Quaternion quaternion2)
104 {
105 throw new NotImplementedException();
106 }
107
108
109 public static void Divide(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
110 {
111 throw new NotImplementedException();
112 }
113
114
115 public static float Dot(Quaternion quaternion1, Quaternion quaternion2)
116 {
117 throw new NotImplementedException();
118 }
119
120
121 public static void Dot(ref Quaternion quaternion1, ref Quaternion quaternion2, out float result)
122 {
123 throw new NotImplementedException();
124 }
125
126
127 public override bool Equals(object obj)
128 {
129 throw new NotImplementedException();
130 }
131
132
133 public bool Equals(Quaternion other)
134 {
135 throw new NotImplementedException();
136 }
137
138
139 public override int GetHashCode()
140 {
141 throw new NotImplementedException();
142 }
143
144
145 public static Quaternion Inverse(Quaternion quaternion)
146 {
147 throw new NotImplementedException();
148 }
149
150
151 public static void Inverse(ref Quaternion quaternion, out Quaternion result)
152 {
153 throw new NotImplementedException();
154 }
155
156
157 public float Length()
158 {
159 //---
160 return (float)Math.Sqrt(Math.Pow(this.W, 2.0) + Math.Pow(this.X, 2.0) + Math.Pow(this.Y, 2.0) + Math.Pow(this.Z, 2.0));
161 //---
162 //throw new NotImplementedException();
163 }
164
165
166 public float LengthSquared()
167 {
168 //---
169 return (float)(Math.Pow(this.W, 2.0) + Math.Pow(this.X, 2.0) + Math.Pow(this.Y, 2.0) + Math.Pow(this.Z, 2.0));
170 //---
171 //throw new NotImplementedException();
172 }
173
174
175 public static Quaternion Lerp(Quaternion quaternion1, Quaternion quaternion2, float amount)
176 {
177 throw new NotImplementedException();
178 }
179
180
181 public static void Lerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result)
182 {
183 throw new NotImplementedException();
184 }
185
186
187 public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, float amount)
188 {
189 throw new NotImplementedException();
190 }
191
192
193 public static void Slerp(ref Quaternion quaternion1, ref Quaternion quaternion2, float amount, out Quaternion result)
194 {
195 throw new NotImplementedException();
196 }
197
198
199 public static Quaternion Subtract(Quaternion quaternion1, Quaternion quaternion2)
200 {
201 throw new NotImplementedException();
202 }
203
204
205 public static void Subtract(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
206 {
207 throw new NotImplementedException();
208 }
209
210
211 public static Quaternion Multiply(Quaternion quaternion1, Quaternion quaternion2)
212 {
213 throw new NotImplementedException();
214 }
215
216
217 public static Quaternion Multiply(Quaternion quaternion1, float scaleFactor)
218 {
219 throw new NotImplementedException();
220 }
221
222
223 public static void Multiply(ref Quaternion quaternion1, float scaleFactor, out Quaternion result)
224 {
225 throw new NotImplementedException();
226 }
227
228
229 public static void Multiply(ref Quaternion quaternion1, ref Quaternion quaternion2, out Quaternion result)
230 {
231 throw new NotImplementedException();
232 }
233
234
235 public static Quaternion Negate(Quaternion quaternion)
236 {
237 throw new NotImplementedException();
238 }
239
240
241 public static void Negate(ref Quaternion quaternion, out Quaternion result)
242 {
243 throw new NotImplementedException();
244 }
245
246
247 public void Normalize()
248 {
249 //---
250 this = Normalize(this);
251 //---
252 //throw new NotImplementedException();
253 }
254
255
256 public static Quaternion Normalize(Quaternion quaternion)
257 {
258 //---
259 return quaternion / quaternion.Length();
260 //---
261 //throw new NotImplementedException();
262 }
263
264
265 public static void Normalize(ref Quaternion quaternion, out Quaternion result)
266 {
267 throw new NotImplementedException();
268 }
269
270
271 public static Quaternion operator +(Quaternion quaternion1, Quaternion quaternion2)
272 {
273 throw new NotImplementedException();
274 }
275
276
277 public static Quaternion operator /(Quaternion quaternion1, Quaternion quaternion2)
278 {
279 throw new NotImplementedException();
280 }
281 public static Quaternion operator /(Quaternion quaternion, float factor)
282 {
283 quaternion.W /= factor;
284 quaternion.X /= factor;
285 quaternion.Y /= factor;
286 quaternion.Z /= factor;
287 return quaternion;
288 }
289
290 public static bool operator ==(Quaternion quaternion1, Quaternion quaternion2)
291 {
292 throw new NotImplementedException();
293 }
294
295
296 public static bool operator !=(Quaternion quaternion1, Quaternion quaternion2)
297 {
298 throw new NotImplementedException();
299 }
300
301
302 public static Quaternion operator *(Quaternion quaternion1, Quaternion quaternion2)
303 {
304 //---
305 //Grassmann product
306 Quaternion quaternionProduct = new Quaternion();
307
308 quaternionProduct.W = quaternion1.W * quaternion2.W - quaternion1.X * quaternion2.X - quaternion1.Y * quaternion2.Y - quaternion1.Z * quaternion2.Z;
309 quaternionProduct.X = quaternion1.W * quaternion2.X + quaternion1.X * quaternion2.W + quaternion1.Y * quaternion2.Z - quaternion1.Z * quaternion2.Y;
310 quaternionProduct.Y = quaternion1.W * quaternion2.Y - quaternion1.X * quaternion2.Z + quaternion1.Y * quaternion2.W + quaternion1.Z * quaternion2.X;
311 quaternionProduct.Z = quaternion1.W * quaternion2.Z + quaternion1.X * quaternion2.Y - quaternion1.Y * quaternion2.X + quaternion1.Z * quaternion2.W;
312 return quaternionProduct;
313 //---
314 //throw new NotImplementedException();
315 }
316
317
318 public static Quaternion operator *(Quaternion quaternion1, float scaleFactor)
319 {
320 throw new NotImplementedException();
321 }
322
323
324 public static Quaternion operator -(Quaternion quaternion1, Quaternion quaternion2)
325 {
326 throw new NotImplementedException();
327 }
328
329
330 public static Quaternion operator -(Quaternion quaternion)
331 {
332 throw new NotImplementedException();
333 }
334
335
336 public override string ToString()
337 {
338 throw new NotImplementedException();
339 }
340
341 private static void Conjugate(ref Quaternion quaternion, out Quaternion result)
342 {
343 throw new NotImplementedException();
344 }
345 }
346}
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector3.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector3.cs
new file mode 100644
index 0000000..fd4bcd2
--- /dev/null
+++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector3.cs
@@ -0,0 +1,620 @@
1#region License
2/*
3MIT License
4Copyright © 2006 The Mono.Xna Team
5
6All rights reserved.
7
8Authors:
9 * Alan McGovern
10
11Permission is hereby granted, free of charge, to any person obtaining a copy
12of this software and associated documentation files (the "Software"), to deal
13in the Software without restriction, including without limitation the rights
14to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
15copies of the Software, and to permit persons to whom the Software is
16furnished to do so, subject to the following conditions:
17
18The above copyright notice and this permission notice shall be included in all
19copies or substantial portions of the Software.
20
21THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
22IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
23FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
24AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
25LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
26OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
27SOFTWARE.
28*/
29#endregion License
30
31using System;
32using System.ComponentModel;
33using System.Text;
34using System.Runtime.InteropServices;
35
36namespace MonoXnaCompactMaths
37{
38 [Serializable]
39 [StructLayout(LayoutKind.Sequential)]
40 //[TypeConverter(typeof(Vector3Converter))]
41 public struct Vector3 : IEquatable<Vector3>
42 {
43 #region Private Fields
44
45 private static Vector3 zero = new Vector3(0f, 0f, 0f);
46 private static Vector3 one = new Vector3(1f, 1f, 1f);
47 private static Vector3 unitX = new Vector3(1f, 0f, 0f);
48 private static Vector3 unitY = new Vector3(0f, 1f, 0f);
49 private static Vector3 unitZ = new Vector3(0f, 0f, 1f);
50 private static Vector3 up = new Vector3(0f, 1f, 0f);
51 private static Vector3 down = new Vector3(0f, -1f, 0f);
52 private static Vector3 right = new Vector3(1f, 0f, 0f);
53 private static Vector3 left = new Vector3(-1f, 0f, 0f);
54 private static Vector3 forward = new Vector3(0f, 0f, -1f);
55 private static Vector3 backward = new Vector3(0f, 0f, 1f);
56
57 #endregion Private Fields
58
59
60 #region Public Fields
61
62 public float X;
63 public float Y;
64 public float Z;
65
66 #endregion Public Fields
67
68
69 #region Properties
70
71 public static Vector3 Zero
72 {
73 get { return zero; }
74 }
75
76 public static Vector3 One
77 {
78 get { return one; }
79 }
80
81 public static Vector3 UnitX
82 {
83 get { return unitX; }
84 }
85
86 public static Vector3 UnitY
87 {
88 get { return unitY; }
89 }
90
91 public static Vector3 UnitZ
92 {
93 get { return unitZ; }
94 }
95
96 public static Vector3 Up
97 {
98 get { return up; }
99 }
100
101 public static Vector3 Down
102 {
103 get { return down; }
104 }
105
106 public static Vector3 Right
107 {
108 get { return right; }
109 }
110
111 public static Vector3 Left
112 {
113 get { return left; }
114 }
115
116 public static Vector3 Forward
117 {
118 get { return forward; }
119 }
120
121 public static Vector3 Backward
122 {
123 get { return backward; }
124 }
125
126 #endregion Properties
127
128
129 #region Constructors
130
131 public Vector3(float x, float y, float z)
132 {
133 this.X = x;
134 this.Y = y;
135 this.Z = z;
136 }
137
138
139 public Vector3(float value)
140 {
141 this.X = value;
142 this.Y = value;
143 this.Z = value;
144 }
145
146
147 /*public Vector3(Vector2 value, float z)
148 {
149 this.X = value.X;
150 this.Y = value.Y;
151 this.Z = z;
152 }*/
153
154
155 #endregion Constructors
156
157
158 #region Public Methods
159
160 public static Vector3 Add(Vector3 value1, Vector3 value2)
161 {
162 value1.X += value2.X;
163 value1.Y += value2.Y;
164 value1.Z += value2.Z;
165 return value1;
166 }
167
168 public static void Add(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
169 {
170 result.X = value1.X + value2.X;
171 result.Y = value1.Y + value2.Y;
172 result.Z = value1.Z + value2.Z;
173 }
174
175 /*public static Vector3 Barycentric(Vector3 value1, Vector3 value2, Vector3 value3, float amount1, float amount2)
176 {
177 return new Vector3(
178 MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2),
179 MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2),
180 MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2));
181 }*/
182
183 /*public static void Barycentric(ref Vector3 value1, ref Vector3 value2, ref Vector3 value3, float amount1, float amount2, out Vector3 result)
184 {
185 result = new Vector3(
186 MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2),
187 MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2),
188 MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2));
189 }*/
190
191 /*public static Vector3 CatmullRom(Vector3 value1, Vector3 value2, Vector3 value3, Vector3 value4, float amount)
192 {
193 return new Vector3(
194 MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),
195 MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount),
196 MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount));
197 }*/
198
199 /*public static void CatmullRom(ref Vector3 value1, ref Vector3 value2, ref Vector3 value3, ref Vector3 value4, float amount, out Vector3 result)
200 {
201 result = new Vector3(
202 MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),
203 MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount),
204 MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount));
205 }*/
206
207 /*public static Vector3 Clamp(Vector3 value1, Vector3 min, Vector3 max)
208 {
209 return new Vector3(
210 MathHelper.Clamp(value1.X, min.X, max.X),
211 MathHelper.Clamp(value1.Y, min.Y, max.Y),
212 MathHelper.Clamp(value1.Z, min.Z, max.Z));
213 }*/
214
215 /*public static void Clamp(ref Vector3 value1, ref Vector3 min, ref Vector3 max, out Vector3 result)
216 {
217 result = new Vector3(
218 MathHelper.Clamp(value1.X, min.X, max.X),
219 MathHelper.Clamp(value1.Y, min.Y, max.Y),
220 MathHelper.Clamp(value1.Z, min.Z, max.Z));
221 }*/
222
223 public static Vector3 Cross(Vector3 vector1, Vector3 vector2)
224 {
225 Cross(ref vector1, ref vector2, out vector1);
226 return vector1;
227 }
228
229 public static void Cross(ref Vector3 vector1, ref Vector3 vector2, out Vector3 result)
230 {
231 result = new Vector3(vector1.Y * vector2.Z - vector2.Y * vector1.Z,
232 -(vector1.X * vector2.Z - vector2.X * vector1.Z),
233 vector1.X * vector2.Y - vector2.X * vector1.Y);
234 }
235
236 public static float Distance(Vector3 vector1, Vector3 vector2)
237 {
238 float result;
239 DistanceSquared(ref vector1, ref vector2, out result);
240 return (float)Math.Sqrt(result);
241 }
242
243 public static void Distance(ref Vector3 value1, ref Vector3 value2, out float result)
244 {
245 DistanceSquared(ref value1, ref value2, out result);
246 result = (float)Math.Sqrt(result);
247 }
248
249 public static float DistanceSquared(Vector3 value1, Vector3 value2)
250 {
251 float result;
252 DistanceSquared(ref value1, ref value2, out result);
253 return result;
254 }
255
256 public static void DistanceSquared(ref Vector3 value1, ref Vector3 value2, out float result)
257 {
258 result = (value1.X - value2.X) * (value1.X - value2.X) +
259 (value1.Y - value2.Y) * (value1.Y - value2.Y) +
260 (value1.Z - value2.Z) * (value1.Z - value2.Z);
261 }
262
263 public static Vector3 Divide(Vector3 value1, Vector3 value2)
264 {
265 value1.X /= value2.X;
266 value1.Y /= value2.Y;
267 value1.Z /= value2.Z;
268 return value1;
269 }
270
271 public static Vector3 Divide(Vector3 value1, float value2)
272 {
273 float factor = 1 / value2;
274 value1.X *= factor;
275 value1.Y *= factor;
276 value1.Z *= factor;
277 return value1;
278 }
279
280 public static void Divide(ref Vector3 value1, float divisor, out Vector3 result)
281 {
282 float factor = 1 / divisor;
283 result.X = value1.X * factor;
284 result.Y = value1.Y * factor;
285 result.Z = value1.Z * factor;
286 }
287
288 public static void Divide(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
289 {
290 result.X = value1.X / value2.X;
291 result.Y = value1.Y / value2.Y;
292 result.Z = value1.Z / value2.Z;
293 }
294
295 public static float Dot(Vector3 vector1, Vector3 vector2)
296 {
297 return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z;
298 }
299
300 public static void Dot(ref Vector3 vector1, ref Vector3 vector2, out float result)
301 {
302 result = vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z;
303 }
304
305 public override bool Equals(object obj)
306 {
307 return (obj is Vector3) ? this == (Vector3)obj : false;
308 }
309
310 public bool Equals(Vector3 other)
311 {
312 return this == other;
313 }
314
315 public override int GetHashCode()
316 {
317 return (int)(this.X + this.Y + this.Z);
318 }
319
320 /*public static Vector3 Hermite(Vector3 value1, Vector3 tangent1, Vector3 value2, Vector3 tangent2, float amount)
321 {
322 Vector3 result = new Vector3();
323 Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result);
324 return result;
325 }*/
326
327 /*public static void Hermite(ref Vector3 value1, ref Vector3 tangent1, ref Vector3 value2, ref Vector3 tangent2, float amount, out Vector3 result)
328 {
329 result.X = MathHelper.Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount);
330 result.Y = MathHelper.Hermite(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount);
331 result.Z = MathHelper.Hermite(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount);
332 }*/
333
334 public float Length()
335 {
336 float result;
337 DistanceSquared(ref this, ref zero, out result);
338 return (float)Math.Sqrt(result);
339 }
340
341 public float LengthSquared()
342 {
343 float result;
344 DistanceSquared(ref this, ref zero, out result);
345 return result;
346 }
347
348 /*public static Vector3 Lerp(Vector3 value1, Vector3 value2, float amount)
349 {
350 return new Vector3(
351 MathHelper.Lerp(value1.X, value2.X, amount),
352 MathHelper.Lerp(value1.Y, value2.Y, amount),
353 MathHelper.Lerp(value1.Z, value2.Z, amount));
354 }*/
355
356 /*public static void Lerp(ref Vector3 value1, ref Vector3 value2, float amount, out Vector3 result)
357 {
358 result = new Vector3(
359 MathHelper.Lerp(value1.X, value2.X, amount),
360 MathHelper.Lerp(value1.Y, value2.Y, amount),
361 MathHelper.Lerp(value1.Z, value2.Z, amount));
362 }*/
363
364 /*public static Vector3 Max(Vector3 value1, Vector3 value2)
365 {
366 return new Vector3(
367 MathHelper.Max(value1.X, value2.X),
368 MathHelper.Max(value1.Y, value2.Y),
369 MathHelper.Max(value1.Z, value2.Z));
370 }*/
371
372 /*public static void Max(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
373 {
374 result = new Vector3(
375 MathHelper.Max(value1.X, value2.X),
376 MathHelper.Max(value1.Y, value2.Y),
377 MathHelper.Max(value1.Z, value2.Z));
378 }*/
379
380 /*public static Vector3 Min(Vector3 value1, Vector3 value2)
381 {
382 return new Vector3(
383 MathHelper.Min(value1.X, value2.X),
384 MathHelper.Min(value1.Y, value2.Y),
385 MathHelper.Min(value1.Z, value2.Z));
386 }*/
387
388 /*public static void Min(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
389 {
390 result = new Vector3(
391 MathHelper.Min(value1.X, value2.X),
392 MathHelper.Min(value1.Y, value2.Y),
393 MathHelper.Min(value1.Z, value2.Z));
394 }*/
395
396 public static Vector3 Multiply(Vector3 value1, Vector3 value2)
397 {
398 value1.X *= value2.X;
399 value1.Y *= value2.Y;
400 value1.Z *= value2.Z;
401 return value1;
402 }
403
404 public static Vector3 Multiply(Vector3 value1, float scaleFactor)
405 {
406 value1.X *= scaleFactor;
407 value1.Y *= scaleFactor;
408 value1.Z *= scaleFactor;
409 return value1;
410 }
411
412 public static void Multiply(ref Vector3 value1, float scaleFactor, out Vector3 result)
413 {
414 result.X = value1.X * scaleFactor;
415 result.Y = value1.Y * scaleFactor;
416 result.Z = value1.Z * scaleFactor;
417 }
418
419 public static void Multiply(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
420 {
421 result.X = value1.X * value2.X;
422 result.Y = value1.Y * value2.Y;
423 result.Z = value1.Z * value2.Z;
424 }
425
426 public static Vector3 Negate(Vector3 value)
427 {
428 value = new Vector3(-value.X, -value.Y, -value.Z);
429 return value;
430 }
431
432 public static void Negate(ref Vector3 value, out Vector3 result)
433 {
434 result = new Vector3(-value.X, -value.Y, -value.Z);
435 }
436
437 public void Normalize()
438 {
439 Normalize(ref this, out this);
440 }
441
442 public static Vector3 Normalize(Vector3 vector)
443 {
444 Normalize(ref vector, out vector);
445 return vector;
446 }
447
448 public static void Normalize(ref Vector3 value, out Vector3 result)
449 {
450 float factor;
451 Distance(ref value, ref zero, out factor);
452 factor = 1f / factor;
453 result.X = value.X * factor;
454 result.Y = value.Y * factor;
455 result.Z = value.Z * factor;
456 }
457
458 public static Vector3 Reflect(Vector3 vector, Vector3 normal)
459 {
460 throw new NotImplementedException();
461 }
462
463 public static void Reflect(ref Vector3 vector, ref Vector3 normal, out Vector3 result)
464 {
465 throw new NotImplementedException();
466 }
467
468 /*public static Vector3 SmoothStep(Vector3 value1, Vector3 value2, float amount)
469 {
470 return new Vector3(
471 MathHelper.SmoothStep(value1.X, value2.X, amount),
472 MathHelper.SmoothStep(value1.Y, value2.Y, amount),
473 MathHelper.SmoothStep(value1.Z, value2.Z, amount));
474 }*/
475
476 /*public static void SmoothStep(ref Vector3 value1, ref Vector3 value2, float amount, out Vector3 result)
477 {
478 result = new Vector3(
479 MathHelper.SmoothStep(value1.X, value2.X, amount),
480 MathHelper.SmoothStep(value1.Y, value2.Y, amount),
481 MathHelper.SmoothStep(value1.Z, value2.Z, amount));
482 }*/
483
484 public static Vector3 Subtract(Vector3 value1, Vector3 value2)
485 {
486 value1.X -= value2.X;
487 value1.Y -= value2.Y;
488 value1.Z -= value2.Z;
489 return value1;
490 }
491
492 public static void Subtract(ref Vector3 value1, ref Vector3 value2, out Vector3 result)
493 {
494 result.X = value1.X - value2.X;
495 result.Y = value1.Y - value2.Y;
496 result.Z = value1.Z - value2.Z;
497 }
498
499 public override string ToString()
500 {
501 StringBuilder sb = new StringBuilder(32);
502 sb.Append("{X:");
503 sb.Append(this.X);
504 sb.Append(" Y:");
505 sb.Append(this.Y);
506 sb.Append(" Z:");
507 sb.Append(this.Z);
508 sb.Append("}");
509 return sb.ToString();
510 }
511
512 public static Vector3 Transform(Vector3 position, Matrix matrix)
513 {
514 Transform(ref position, ref matrix, out position);
515 return position;
516 }
517
518 public static void Transform(ref Vector3 position, ref Matrix matrix, out Vector3 result)
519 {
520 result = new Vector3((position.X * matrix.M11) + (position.Y * matrix.M21) + (position.Z * matrix.M31) + matrix.M41,
521 (position.X * matrix.M12) + (position.Y * matrix.M22) + (position.Z * matrix.M32) + matrix.M42,
522 (position.X * matrix.M13) + (position.Y * matrix.M23) + (position.Z * matrix.M33) + matrix.M43);
523 }
524
525 public static Vector3 TransformNormal(Vector3 normal, Matrix matrix)
526 {
527 TransformNormal(ref normal, ref matrix, out normal);
528 return normal;
529 }
530
531 public static void TransformNormal(ref Vector3 normal, ref Matrix matrix, out Vector3 result)
532 {
533 result = new Vector3((normal.X * matrix.M11) + (normal.Y * matrix.M21) + (normal.Z * matrix.M31),
534 (normal.X * matrix.M12) + (normal.Y * matrix.M22) + (normal.Z * matrix.M32),
535 (normal.X * matrix.M13) + (normal.Y * matrix.M23) + (normal.Z * matrix.M33));
536 }
537
538 #endregion Public methods
539
540
541 #region Operators
542
543 public static bool operator ==(Vector3 value1, Vector3 value2)
544 {
545 return value1.X == value2.X
546 && value1.Y == value2.Y
547 && value1.Z == value2.Z;
548 }
549
550 public static bool operator !=(Vector3 value1, Vector3 value2)
551 {
552 return !(value1 == value2);
553 }
554
555 public static Vector3 operator +(Vector3 value1, Vector3 value2)
556 {
557 value1.X += value2.X;
558 value1.Y += value2.Y;
559 value1.Z += value2.Z;
560 return value1;
561 }
562
563 public static Vector3 operator -(Vector3 value)
564 {
565 value = new Vector3(-value.X, -value.Y, -value.Z);
566 return value;
567 }
568
569 public static Vector3 operator -(Vector3 value1, Vector3 value2)
570 {
571 value1.X -= value2.X;
572 value1.Y -= value2.Y;
573 value1.Z -= value2.Z;
574 return value1;
575 }
576
577 public static Vector3 operator *(Vector3 value1, Vector3 value2)
578 {
579 value1.X *= value2.X;
580 value1.Y *= value2.Y;
581 value1.Z *= value2.Z;
582 return value1;
583 }
584
585 public static Vector3 operator *(Vector3 value, float scaleFactor)
586 {
587 value.X *= scaleFactor;
588 value.Y *= scaleFactor;
589 value.Z *= scaleFactor;
590 return value;
591 }
592
593 public static Vector3 operator *(float scaleFactor, Vector3 value)
594 {
595 value.X *= scaleFactor;
596 value.Y *= scaleFactor;
597 value.Z *= scaleFactor;
598 return value;
599 }
600
601 public static Vector3 operator /(Vector3 value1, Vector3 value2)
602 {
603 value1.X /= value2.X;
604 value1.Y /= value2.Y;
605 value1.Z /= value2.Z;
606 return value1;
607 }
608
609 public static Vector3 operator /(Vector3 value, float divider)
610 {
611 float factor = 1 / divider;
612 value.X *= factor;
613 value.Y *= factor;
614 value.Z *= factor;
615 return value;
616 }
617
618 #endregion
619 }
620}
diff --git a/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector4.cs b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector4.cs
new file mode 100644
index 0000000..c75b7c8
--- /dev/null
+++ b/libraries/ModifiedBulletX/MonoXnaCompactMaths/Vector4.cs
@@ -0,0 +1,630 @@
1#region License
2/*
3MIT License
4Copyright © 2006 The Mono.Xna Team
5
6All rights reserved.
7
8Permission is hereby granted, free of charge, to any person obtaining a copy
9of this software and associated documentation files (the "Software"), to deal
10in the Software without restriction, including without limitation the rights
11to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
12copies of the Software, and to permit persons to whom the Software is
13furnished to do so, subject to the following conditions:
14
15The above copyright notice and this permission notice shall be included in all
16copies or substantial portions of the Software.
17
18THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
19IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
20FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
21AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
22LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
23OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
24SOFTWARE.
25*/
26#endregion License
27
28using System;
29using System.ComponentModel;
30using System.Text;
31using System.Runtime.InteropServices;
32
33namespace MonoXnaCompactMaths
34{
35 [Serializable]
36 [StructLayout(LayoutKind.Sequential)]
37 //[TypeConverter(typeof(Vector4Converter))]
38 public struct Vector4 : IEquatable<Vector4>
39 {
40 #region Private Fields
41
42 private static Vector4 zeroVector = new Vector4();
43 private static Vector4 unitVector = new Vector4(1f, 1f, 1f, 1f);
44 private static Vector4 unitXVector = new Vector4(1f, 0f, 0f, 0f);
45 private static Vector4 unitYVector = new Vector4(0f, 1f, 0f, 0f);
46 private static Vector4 unitZVector = new Vector4(0f, 0f, 1f, 0f);
47 private static Vector4 unitWVector = new Vector4(0f, 0f, 0f, 1f);
48
49 #endregion Private Fields
50
51
52 #region Public Fields
53
54 public float X;
55 public float Y;
56 public float Z;
57 public float W;
58
59 #endregion Public Fields
60
61
62 #region Properties
63
64 public static Vector4 Zero
65 {
66 get { return zeroVector; }
67 }
68
69 public static Vector4 One
70 {
71 get { return unitVector; }
72 }
73
74 public static Vector4 UnitX
75 {
76 get { return unitXVector; }
77 }
78
79 public static Vector4 UnitY
80 {
81 get { return unitYVector; }
82 }
83
84 public static Vector4 UnitZ
85 {
86 get { return unitZVector; }
87 }
88
89 public static Vector4 UnitW
90 {
91 get { return unitWVector; }
92 }
93
94 #endregion Properties
95
96
97 #region Constructors
98
99 public Vector4(float x, float y, float z, float w)
100 {
101 this.X = x;
102 this.Y = y;
103 this.Z = z;
104 this.W = w;
105 }
106
107 /*public Vector4(Vector2 value, float z, float w)
108 {
109 this.X = value.X;
110 this.Y = value.Y;
111 this.Z = z;
112 this.W = w;
113 }*/
114
115 public Vector4(Vector3 value, float w)
116 {
117 this.X = value.X;
118 this.Y = value.Y;
119 this.Z = value.Z;
120 this.W = w;
121 }
122
123 public Vector4(float value)
124 {
125 this.X = value;
126 this.Y = value;
127 this.Z = value;
128 this.W = value;
129 }
130
131 #endregion
132
133
134 #region Public Methods
135
136 public static Vector4 Add(Vector4 value1, Vector4 value2)
137 {
138 value1.W += value2.W;
139 value1.X += value2.X;
140 value1.Y += value2.Y;
141 value1.Z += value2.Z;
142 return value1;
143 }
144
145 public static void Add(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
146 {
147 result.W = value1.W + value2.W;
148 result.X = value1.X + value2.X;
149 result.Y = value1.Y + value2.Y;
150 result.Z = value1.Z + value2.Z;
151 }
152
153 /*public static Vector4 Barycentric(Vector4 value1, Vector4 value2, Vector4 value3, float amount1, float amount2)
154 {
155 return new Vector4(
156 MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2),
157 MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2),
158 MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2),
159 MathHelper.Barycentric(value1.W, value2.W, value3.W, amount1, amount2));
160 }*/
161
162 /*public static void Barycentric(ref Vector4 value1, ref Vector4 value2, ref Vector4 value3, float amount1, float amount2, out Vector4 result)
163 {
164 result = new Vector4(
165 MathHelper.Barycentric(value1.X, value2.X, value3.X, amount1, amount2),
166 MathHelper.Barycentric(value1.Y, value2.Y, value3.Y, amount1, amount2),
167 MathHelper.Barycentric(value1.Z, value2.Z, value3.Z, amount1, amount2),
168 MathHelper.Barycentric(value1.W, value2.W, value3.W, amount1, amount2));
169 }*/
170
171 /*public static Vector4 CatmullRom(Vector4 value1, Vector4 value2, Vector4 value3, Vector4 value4, float amount)
172 {
173 return new Vector4(
174 MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),
175 MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount),
176 MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount),
177 MathHelper.CatmullRom(value1.W, value2.W, value3.W, value4.W, amount));
178 }*/
179
180 /*public static void CatmullRom(ref Vector4 value1, ref Vector4 value2, ref Vector4 value3, ref Vector4 value4, float amount, out Vector4 result)
181 {
182 result = new Vector4(
183 MathHelper.CatmullRom(value1.X, value2.X, value3.X, value4.X, amount),
184 MathHelper.CatmullRom(value1.Y, value2.Y, value3.Y, value4.Y, amount),
185 MathHelper.CatmullRom(value1.Z, value2.Z, value3.Z, value4.Z, amount),
186 MathHelper.CatmullRom(value1.W, value2.W, value3.W, value4.W, amount));
187 }*/
188
189 /*public static Vector4 Clamp(Vector4 value1, Vector4 min, Vector4 max)
190 {
191 return new Vector4(
192 MathHelper.Clamp(value1.X, min.X, max.X),
193 MathHelper.Clamp(value1.Y, min.Y, max.Y),
194 MathHelper.Clamp(value1.Z, min.Z, max.Z),
195 MathHelper.Clamp(value1.W, min.W, max.W));
196 }*/
197
198 /*public static void Clamp(ref Vector4 value1, ref Vector4 min, ref Vector4 max, out Vector4 result)
199 {
200 result = new Vector4(
201 MathHelper.Clamp(value1.X, min.X, max.X),
202 MathHelper.Clamp(value1.Y, min.Y, max.Y),
203 MathHelper.Clamp(value1.Z, min.Z, max.Z),
204 MathHelper.Clamp(value1.W, min.W, max.W));
205 }*/
206
207 public static float Distance(Vector4 value1, Vector4 value2)
208 {
209 return (float)Math.Sqrt(DistanceSquared(value1, value2));
210 }
211
212 public static void Distance(ref Vector4 value1, ref Vector4 value2, out float result)
213 {
214 result = (float)Math.Sqrt(DistanceSquared(value1, value2));
215 }
216
217 public static float DistanceSquared(Vector4 value1, Vector4 value2)
218 {
219 float result;
220 DistanceSquared(ref value1, ref value2, out result);
221 return result;
222 }
223
224 public static void DistanceSquared(ref Vector4 value1, ref Vector4 value2, out float result)
225 {
226 result = (value1.W - value2.W) * (value1.W - value2.W) +
227 (value1.X - value2.X) * (value1.X - value2.X) +
228 (value1.Y - value2.Y) * (value1.Y - value2.Y) +
229 (value1.Z - value2.Z) * (value1.Z - value2.Z);
230 }
231
232 public static Vector4 Divide(Vector4 value1, Vector4 value2)
233 {
234 value1.W /= value2.W;
235 value1.X /= value2.X;
236 value1.Y /= value2.Y;
237 value1.Z /= value2.Z;
238 return value1;
239 }
240
241 public static Vector4 Divide(Vector4 value1, float divider)
242 {
243 float factor = 1f / divider;
244 value1.W *= factor;
245 value1.X *= factor;
246 value1.Y *= factor;
247 value1.Z *= factor;
248 return value1;
249 }
250
251 public static void Divide(ref Vector4 value1, float divider, out Vector4 result)
252 {
253 float factor = 1f / divider;
254 result.W = value1.W * factor;
255 result.X = value1.X * factor;
256 result.Y = value1.Y * factor;
257 result.Z = value1.Z * factor;
258 }
259
260 public static void Divide(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
261 {
262 result.W = value1.W / value2.W;
263 result.X = value1.X / value2.X;
264 result.Y = value1.Y / value2.Y;
265 result.Z = value1.Z / value2.Z;
266 }
267
268 public static float Dot(Vector4 vector1, Vector4 vector2)
269 {
270 return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z + vector1.W * vector2.W;
271 }
272
273 public static void Dot(ref Vector4 vector1, ref Vector4 vector2, out float result)
274 {
275 result = vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z + vector1.W * vector2.W;
276 }
277
278 public override bool Equals(object obj)
279 {
280 return (obj is Vector4) ? this == (Vector4)obj : false;
281 }
282
283 public bool Equals(Vector4 other)
284 {
285 return this.W == other.W
286 && this.X == other.X
287 && this.Y == other.Y
288 && this.Z == other.Z;
289 }
290
291 public override int GetHashCode()
292 {
293 return (int)(this.W + this.X + this.Y + this.Y);
294 }
295
296 /*public static Vector4 Hermite(Vector4 value1, Vector4 tangent1, Vector4 value2, Vector4 tangent2, float amount)
297 {
298 Vector4 result = new Vector4();
299 Hermite(ref value1, ref tangent1, ref value2, ref tangent2, amount, out result);
300 return result;
301 }*/
302
303 /*public static void Hermite(ref Vector4 value1, ref Vector4 tangent1, ref Vector4 value2, ref Vector4 tangent2, float amount, out Vector4 result)
304 {
305 result.W = MathHelper.Hermite(value1.W, tangent1.W, value2.W, tangent2.W, amount);
306 result.X = MathHelper.Hermite(value1.X, tangent1.X, value2.X, tangent2.X, amount);
307 result.Y = MathHelper.Hermite(value1.Y, tangent1.Y, value2.Y, tangent2.Y, amount);
308 result.Z = MathHelper.Hermite(value1.Z, tangent1.Z, value2.Z, tangent2.Z, amount);
309 }*/
310
311 public float Length()
312 {
313 float result;
314 DistanceSquared(ref this, ref zeroVector, out result);
315 return (float)Math.Sqrt(result);
316 }
317
318 public float LengthSquared()
319 {
320 float result;
321 DistanceSquared(ref this, ref zeroVector, out result);
322 return result;
323 }
324
325 /*public static Vector4 Lerp(Vector4 value1, Vector4 value2, float amount)
326 {
327 return new Vector4(
328 MathHelper.Lerp(value1.X, value2.X, amount),
329 MathHelper.Lerp(value1.Y, value2.Y, amount),
330 MathHelper.Lerp(value1.Z, value2.Z, amount),
331 MathHelper.Lerp(value1.W, value2.W, amount));
332 }*/
333
334 /*public static void Lerp(ref Vector4 value1, ref Vector4 value2, float amount, out Vector4 result)
335 {
336 result = new Vector4(
337 MathHelper.Lerp(value1.X, value2.X, amount),
338 MathHelper.Lerp(value1.Y, value2.Y, amount),
339 MathHelper.Lerp(value1.Z, value2.Z, amount),
340 MathHelper.Lerp(value1.W, value2.W, amount));
341 }*/
342
343 /*public static Vector4 Max(Vector4 value1, Vector4 value2)
344 {
345 return new Vector4(
346 MathHelper.Max(value1.X, value2.X),
347 MathHelper.Max(value1.Y, value2.Y),
348 MathHelper.Max(value1.Z, value2.Z),
349 MathHelper.Max(value1.W, value2.W));
350 }*/
351
352 /*public static void Max(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
353 {
354 result = new Vector4(
355 MathHelper.Max(value1.X, value2.X),
356 MathHelper.Max(value1.Y, value2.Y),
357 MathHelper.Max(value1.Z, value2.Z),
358 MathHelper.Max(value1.W, value2.W));
359 }*/
360
361 /*public static Vector4 Min(Vector4 value1, Vector4 value2)
362 {
363 return new Vector4(
364 MathHelper.Min(value1.X, value2.X),
365 MathHelper.Min(value1.Y, value2.Y),
366 MathHelper.Min(value1.Z, value2.Z),
367 MathHelper.Min(value1.W, value2.W));
368 }*/
369
370 /*public static void Min(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
371 {
372 result = new Vector4(
373 MathHelper.Min(value1.X, value2.X),
374 MathHelper.Min(value1.Y, value2.Y),
375 MathHelper.Min(value1.Z, value2.Z),
376 MathHelper.Min(value1.W, value2.W));
377 }*/
378
379 public static Vector4 Multiply(Vector4 value1, Vector4 value2)
380 {
381 value1.W *= value2.W;
382 value1.X *= value2.X;
383 value1.Y *= value2.Y;
384 value1.Z *= value2.Z;
385 return value1;
386 }
387
388 public static Vector4 Multiply(Vector4 value1, float scaleFactor)
389 {
390 value1.W *= scaleFactor;
391 value1.X *= scaleFactor;
392 value1.Y *= scaleFactor;
393 value1.Z *= scaleFactor;
394 return value1;
395 }
396
397 public static void Multiply(ref Vector4 value1, float scaleFactor, out Vector4 result)
398 {
399 result.W = value1.W * scaleFactor;
400 result.X = value1.X * scaleFactor;
401 result.Y = value1.Y * scaleFactor;
402 result.Z = value1.Z * scaleFactor;
403 }
404
405 public static void Multiply(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
406 {
407 result.W = value1.W * value2.W;
408 result.X = value1.X * value2.X;
409 result.Y = value1.Y * value2.Y;
410 result.Z = value1.Z * value2.Z;
411 }
412
413 public static Vector4 Negate(Vector4 value)
414 {
415 value = new Vector4(-value.X, -value.Y, -value.Z, -value.W);
416 return value;
417 }
418
419 public static void Negate(ref Vector4 value, out Vector4 result)
420 {
421 result = new Vector4(-value.X, -value.Y, -value.Z,-value.W);
422 }
423
424 public void Normalize()
425 {
426 Normalize(ref this, out this);
427 }
428
429 public static Vector4 Normalize(Vector4 vector)
430 {
431 Normalize(ref vector, out vector);
432 return vector;
433 }
434
435 public static void Normalize(ref Vector4 vector, out Vector4 result)
436 {
437 float factor;
438 DistanceSquared(ref vector, ref zeroVector, out factor);
439 factor = 1f / (float)Math.Sqrt(factor);
440
441 result.W = vector.W * factor;
442 result.X = vector.X * factor;
443 result.Y = vector.Y * factor;
444 result.Z = vector.Z * factor;
445 }
446
447 /*public static Vector4 SmoothStep(Vector4 value1, Vector4 value2, float amount)
448 {
449 return new Vector4(
450 MathHelper.SmoothStep(value1.X, value2.X, amount),
451 MathHelper.SmoothStep(value1.Y, value2.Y, amount),
452 MathHelper.SmoothStep(value1.Z, value2.Z, amount),
453 MathHelper.SmoothStep(value1.W, value2.W, amount));
454 }*/
455
456 /*public static void SmoothStep(ref Vector4 value1, ref Vector4 value2, float amount, out Vector4 result)
457 {
458 result = new Vector4(
459 MathHelper.SmoothStep(value1.X, value2.X, amount),
460 MathHelper.SmoothStep(value1.Y, value2.Y, amount),
461 MathHelper.SmoothStep(value1.Z, value2.Z, amount),
462 MathHelper.SmoothStep(value1.W, value2.W, amount));
463 }*/
464
465 public static Vector4 Subtract(Vector4 value1, Vector4 value2)
466 {
467 value1.W -= value2.W;
468 value1.X -= value2.X;
469 value1.Y -= value2.Y;
470 value1.Z -= value2.Z;
471 return value1;
472 }
473
474 public static void Subtract(ref Vector4 value1, ref Vector4 value2, out Vector4 result)
475 {
476 result.W = value1.W - value2.W;
477 result.X = value1.X - value2.X;
478 result.Y = value1.Y - value2.Y;
479 result.Z = value1.Z - value2.Z;
480 }
481
482 /*public static Vector4 Transform(Vector2 position, Matrix matrix)
483 {
484 Vector4 result;
485 Transform(ref position, ref matrix, out result);
486 return result;
487 }*/
488
489 public static Vector4 Transform(Vector3 position, Matrix matrix)
490 {
491 Vector4 result;
492 Transform(ref position, ref matrix, out result);
493 return result;
494 }
495
496 public static Vector4 Transform(Vector4 vector, Matrix matrix)
497 {
498 Transform(ref vector, ref matrix, out vector);
499 return vector;
500 }
501
502 /*public static void Transform(ref Vector2 position, ref Matrix matrix, out Vector4 result)
503 {
504 result = new Vector4((position.X * matrix.M11) + (position.Y * matrix.M21) + matrix.M41,
505 (position.X * matrix.M12) + (position.Y * matrix.M22) + matrix.M42,
506 (position.X * matrix.M13) + (position.Y * matrix.M23) + matrix.M43,
507 (position.X * matrix.M14) + (position.Y * matrix.M24) + matrix.M44);
508 }*/
509
510 public static void Transform(ref Vector3 position, ref Matrix matrix, out Vector4 result)
511 {
512 result = new Vector4((position.X * matrix.M11) + (position.Y * matrix.M21) + (position.Z * matrix.M31) + matrix.M41,
513 (position.X * matrix.M12) + (position.Y * matrix.M22) + (position.Z * matrix.M32) + matrix.M42,
514 (position.X * matrix.M13) + (position.Y * matrix.M23) + (position.Z * matrix.M33) + matrix.M43,
515 (position.X * matrix.M14) + (position.Y * matrix.M24) + (position.Z * matrix.M34) + matrix.M44);
516 }
517
518 public static void Transform(ref Vector4 vector, ref Matrix matrix, out Vector4 result)
519 {
520 result = new Vector4((vector.X * matrix.M11) + (vector.Y * matrix.M21) + (vector.Z * matrix.M31) + (vector.W * matrix.M41),
521 (vector.X * matrix.M12) + (vector.Y * matrix.M22) + (vector.Z * matrix.M32) + (vector.W * matrix.M42),
522 (vector.X * matrix.M13) + (vector.Y * matrix.M23) + (vector.Z * matrix.M33) + (vector.W * matrix.M43),
523 (vector.X * matrix.M14) + (vector.Y * matrix.M24) + (vector.Z * matrix.M34) + (vector.W * matrix.M44));
524 }
525
526 public override string ToString()
527 {
528 StringBuilder sb = new StringBuilder(32);
529 sb.Append("{X:");
530 sb.Append(this.X);
531 sb.Append(" Y:");
532 sb.Append(this.Y);
533 sb.Append(" Z:");
534 sb.Append(this.Z);
535 sb.Append(" W:");
536 sb.Append(this.W);
537 sb.Append("}");
538 return sb.ToString();
539 }
540
541 #endregion Public Methods
542
543
544 #region Operators
545
546 public static Vector4 operator -(Vector4 value)
547 {
548 return new Vector4(-value.X, -value.Y, -value.Z, -value.W);
549 }
550
551 public static bool operator ==(Vector4 value1, Vector4 value2)
552 {
553 return value1.W == value2.W
554 && value1.X == value2.X
555 && value1.Y == value2.Y
556 && value1.Z == value2.Z;
557 }
558
559 public static bool operator !=(Vector4 value1, Vector4 value2)
560 {
561 return !(value1 == value2);
562 }
563
564 public static Vector4 operator +(Vector4 value1, Vector4 value2)
565 {
566 value1.W += value2.W;
567 value1.X += value2.X;
568 value1.Y += value2.Y;
569 value1.Z += value2.Z;
570 return value1;
571 }
572
573 public static Vector4 operator -(Vector4 value1, Vector4 value2)
574 {
575 value1.W -= value2.W;
576 value1.X -= value2.X;
577 value1.Y -= value2.Y;
578 value1.Z -= value2.Z;
579 return value1;
580 }
581
582 public static Vector4 operator *(Vector4 value1, Vector4 value2)
583 {
584 value1.W *= value2.W;
585 value1.X *= value2.X;
586 value1.Y *= value2.Y;
587 value1.Z *= value2.Z;
588 return value1;
589 }
590
591 public static Vector4 operator *(Vector4 value1, float scaleFactor)
592 {
593 value1.W *= scaleFactor;
594 value1.X *= scaleFactor;
595 value1.Y *= scaleFactor;
596 value1.Z *= scaleFactor;
597 return value1;
598 }
599
600 public static Vector4 operator *(float scaleFactor, Vector4 value1)
601 {
602 value1.W *= scaleFactor;
603 value1.X *= scaleFactor;
604 value1.Y *= scaleFactor;
605 value1.Z *= scaleFactor;
606 return value1;
607 }
608
609 public static Vector4 operator /(Vector4 value1, Vector4 value2)
610 {
611 value1.W /= value2.W;
612 value1.X /= value2.X;
613 value1.Y /= value2.Y;
614 value1.Z /= value2.Z;
615 return value1;
616 }
617
618 public static Vector4 operator /(Vector4 value1, float divider)
619 {
620 float factor = 1f / divider;
621 value1.W *= factor;
622 value1.X *= factor;
623 value1.Y *= factor;
624 value1.Z *= factor;
625 return value1;
626 }
627
628 #endregion Operators
629 }
630}